#include "PdecConfig.h"

#include "fsfw/filesystem/HasFileSystemIF.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "pdecconfigdefs.h"

PdecConfig::PdecConfig() : localParameterHandler("conf/pdecconfig", SdCardManager::instance()) {}

PdecConfig::~PdecConfig() {}

void PdecConfig::setMemoryBaseAddress(uint32_t* memoryBaseAddress_) {
  memoryBaseAddress = memoryBaseAddress_;
}

ReturnValue_t PdecConfig::write() {
  if (memoryBaseAddress == nullptr) {
    sif::error << "PdecConfig::write: Memory base address not set" << std::endl;
    return returnvalue::FAILED;
  }
  ReturnValue_t result = initializePersistentParameters();
  if (result != returnvalue::OK) {
  	return result;
  }
  result = writeFrameHeaderFirstOctet();
  if (result != returnvalue::OK) {
		return result;
	}
  result = writeFrameHeaderSecondOctet();
  if (result != returnvalue::OK) {
		return result;
	}
  writeMapConfig();
  return returnvalue::FAILED;
}

ReturnValue_t PdecConfig::initializePersistentParameters() {
  ReturnValue_t result = localParameterHandler.initialize();
  if (result != returnvalue::OK) {
    if (result == HasFileSystemIF::FILE_DOES_NOT_EXIST) {
      result = createPersistentConfig();
      if (result != returnvalue::OK) {
        return result;
      }
    }
  }
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::createPersistentConfig() {
  ReturnValue_t result = localParameterHandler.addParameter(
      pdecconfigdefs::paramkeys::POSITIVE_WINDOW, pdecconfigdefs::defaultvalue::positiveWindow);
  if (result != returnvalue::OK) {
    sif::error << "PdecConfig::createPersistentConfig: Failed to set positive window" << std::endl;
    return result;
  }
  ReturnValue_t result = localParameterHandler.addParameter(
      pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, pdecconfigdefs::defaultvalue::negativeWindow);
  if (result != returnvalue::OK) {
    sif::error << "PdecConfig::createPersistentConfig: Failed to set negative window" << std::endl;
    return result;
  }
  return returnvalue::OK;
}

uint32_t PdecConfig::getImrReg() {
  return static_cast<uint32_t>(enableNewFarIrq << 2) |
         static_cast<uint32_t>(enableTcAbortIrq << 1) | static_cast<uint32_t>(enableTcNewIrq);
}

ReturnValue_t PdecConfig::setPositiveWindow(uint8_t pw) {
  if (memoryBaseAddress == nullptr) {
    sif::error << "PdecConfig::setPositiveWindow: Memory base address not set" << std::endl;
    return returnvalue::FAILED;
  }
  ReturnValue_t result =
      localParameterHandler.updateParameter(pdecconfigdefs::paramkeys::POSITIVE_WINDOW, pw);
  if (result != returnvalue::OK) {
    return result;
  }
  // Rewrite second config word which contains the positive window parameter
  writeFrameHeaderSecondOctet();
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::setNegativeWindow(uint8_t nw) {
  if (memoryBaseAddress == nullptr) {
    sif::error << "PdecConfig::setPositiveWindow: Memory base address not set" << std::endl;
    return returnvalue::FAILED;
  }
  ReturnValue_t result =
      localParameterHandler.updateParameter(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, nw);
  if (result != returnvalue::OK) {
    return result;
  }
  // Rewrite second config word which contains the negative window parameter
  writeFrameHeaderSecondOctet();
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::getPositiveWindow(uint8_t& positiveWindow) {
  ReturnValue_t result =
      localParameterHandler.getValue(pdecconfigdefs::paramkeys::POSITIVE_WINDOW, positiveWindow);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::getNegativeWindow(uint8_t& negativeWindow) {
  ReturnValue_t result =
      localParameterHandler.getValue(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, negativeWindow);
  if (result != returnvalue::OK) {
    return result;
  }
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::writeFrameHeaderFirstOctet() {
  uint32_t word = 0;
  word |= (VERSION_ID << 30);

  // Setting the bypass flag and the control command flag should not have any
  // implication on the operation of the PDEC IP Core
  word |= (BYPASS_FLAG << 29);
  word |= (CONTROL_COMMAND_FLAG << 28);

  word |= (RESERVED_FIELD_A << 26);
  word |= (SPACECRAFT_ID << 16);
  word |= (VIRTUAL_CHANNEL << 10);
  word |= (DUMMY_BITS << 8);
  uint8_t negativeWindow = 0;
  ReturnValue_t result =
      localParameterHandler.getValue(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, negativeWindow);
  if (result != returnvalue::OK) {
    return result;
  }
  word |= negativeWindow;
  *(memoryBaseAddress + FRAME_HEADER_OFFSET) = word;
  return returnvalue::OK;
}

ReturnValue_t PdecConfig::writeFrameHeaderSecondOctet() {
  uint8_t negativeWindow = 0;
  ReturnValue_t result =
      localParameterHandler.getValue(pdecconfigdefs::paramkeys::NEGATIVE_WINDOW, negativeWindow);
  if (result != returnvalue::OK) {
    return result;
  }
  uint32_t word = 0;
  word = 0;
  word |= (negativeWindow << 24);
  word |= (HIGH_AU_MAP_ID << 16);
  word |= (ENABLE_DERANDOMIZER << 8);
  *(memoryBaseAddress + FRAME_HEADER_OFFSET + 1) = word;
  return returnvalue::OK;
}

void PdecConfig::writeMapConfig() {
  // Configure all MAP IDs as invalid
  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
    *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + idx / 4) =
        NO_DESTINATION << 24 | NO_DESTINATION << 16 | NO_DESTINATION << 8 | NO_DESTINATION;
  }

  // All TCs with MAP ID 7 will be routed to the PM module (can then be read from memory)
  uint8_t routeToPm = calcMapAddrEntry(PM_BUFFER);
  *(memoryBaseAddress + MAP_ADDR_LUT_OFFSET + 1) =
      (NO_DESTINATION << 24) | (NO_DESTINATION << 16) | (NO_DESTINATION << 8) | routeToPm;

  // Write map id clock frequencies
  for (int idx = 0; idx <= MAX_MAP_ADDR; idx += 4) {
    *(memoryBaseAddress + MAP_CLK_FREQ_OFFSET + idx / 4) =
        MAP_CLK_FREQ << 24 | MAP_CLK_FREQ << 16 | MAP_CLK_FREQ << 8 | MAP_CLK_FREQ;
  }
}

uint8_t PdecConfig::calcMapAddrEntry(uint8_t moduleId) {
  uint8_t lutEntry = 0;
  uint8_t parity = getOddParity(moduleId | (1 << VALID_POSITION));
  lutEntry = (parity << PARITY_POSITION) | (1 << VALID_POSITION) | moduleId;
  return lutEntry;
}

uint8_t PdecConfig::getOddParity(uint8_t number) {
  uint8_t parityBit = 0;
  uint8_t countBits = 0;
  for (unsigned int idx = 0; idx < sizeof(number) * 8; idx++) {
    countBits += (number >> idx) & 0x1;
  }
  parityBit = ~(countBits & 0x1) & 0x1;
  return parityBit;
}