#include "Max31865PT1000Handler.h"

#include <bitset>
#include <cmath>

#include "fsfw/datapool/PoolReadGuard.h"

Max31865PT1000Handler::Max31865PT1000Handler(object_id_t objectId, object_id_t comIF,
                                             CookieIF *comCookie)
    : DeviceHandlerBase(objectId, comIF, comCookie),
      sensorDataset(this, MAX31865::REQUEST_RTD),
      sensorDatasetSid(sensorDataset.getSid()) {
#if OBSW_VERBOSE_LEVEL >= 1
  debugDivider = new PeriodicOperationDivider(10);
#endif
}

Max31865PT1000Handler::~Max31865PT1000Handler() {}

void Max31865PT1000Handler::doStartUp() {
  if (internalState == InternalState::NONE) {
    internalState = InternalState::WARMUP;
    Clock::getUptime(&startTime);
  }

  if (internalState == InternalState::WARMUP) {
    dur_millis_t timeNow = 0;
    Clock::getUptime(&timeNow);
    if (timeNow - startTime >= 100) {
      internalState = InternalState::CONFIGURE;
    }
  }

  if (internalState == InternalState::CONFIGURE) {
    if (commandExecuted) {
      commandExecuted = false;
      internalState = InternalState::REQUEST_CONFIG;
    }
  }

  if (internalState == InternalState::REQUEST_CONFIG) {
    if (commandExecuted) {
      commandExecuted = false;
      internalState = InternalState::CONFIG_HIGH_THRESHOLD;
    }
  }

  if (internalState == InternalState::CONFIG_HIGH_THRESHOLD) {
    if (commandExecuted) {
      internalState = InternalState::REQUEST_HIGH_THRESHOLD;
      commandExecuted = false;
    }
  }

  if (internalState == InternalState::REQUEST_HIGH_THRESHOLD) {
    if (commandExecuted) {
      internalState = InternalState::CONFIG_LOW_THRESHOLD;
      commandExecuted = false;
    }
  }

  if (internalState == InternalState::CONFIG_LOW_THRESHOLD) {
    if (commandExecuted) {
      internalState = InternalState::REQUEST_LOW_THRESHOLD;
      commandExecuted = false;
    }
  }

  if (internalState == InternalState::REQUEST_LOW_THRESHOLD) {
    if (commandExecuted) {
      internalState = InternalState::CLEAR_FAULT_BYTE;
      commandExecuted = false;
    }
  }
  if (internalState == InternalState::CLEAR_FAULT_BYTE) {
    if (commandExecuted) {
      commandExecuted = false;
      internalState = InternalState::RUNNING;
      if (instantNormal) {
        setMode(MODE_NORMAL);
      } else {
        setMode(_MODE_TO_ON);
      }
    }
  }
}

void Max31865PT1000Handler::doShutDown() {
  commandExecuted = false;
  warningSwitch = true;
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t Max31865PT1000Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  if (internalState == InternalState::RUNNING) {
    *id = MAX31865::REQUEST_RTD;
    return buildCommandFromCommand(*id, nullptr, 0);
  } else if (internalState == InternalState::REQUEST_FAULT_BYTE) {
    *id = MAX31865::REQUEST_FAULT_BYTE;
    return buildCommandFromCommand(*id, nullptr, 0);
  } else if (internalState == InternalState::CLEAR_FAULT_BYTE) {
    *id = MAX31865::CLEAR_FAULT_BYTE;
    return buildCommandFromCommand(*id, nullptr, 0);
  } else {
    return DeviceHandlerBase::NOTHING_TO_SEND;
  }
}

ReturnValue_t Max31865PT1000Handler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  switch (internalState) {
    case (InternalState::NONE):
    case (InternalState::WARMUP):
    case (InternalState::RUNNING):
      return DeviceHandlerBase::NOTHING_TO_SEND;
    case (InternalState::CONFIGURE): {
      *id = MAX31865::CONFIG_CMD;
      uint8_t config[1] = {DEFAULT_CONFIG};
      return buildCommandFromCommand(*id, config, 1);
    }
    case (InternalState::REQUEST_CONFIG): {
      *id = MAX31865::REQUEST_CONFIG;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (InternalState::CONFIG_HIGH_THRESHOLD): {
      *id = MAX31865::WRITE_HIGH_THRESHOLD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (InternalState::REQUEST_HIGH_THRESHOLD): {
      *id = MAX31865::REQUEST_HIGH_THRESHOLD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (InternalState::CONFIG_LOW_THRESHOLD): {
      *id = MAX31865::WRITE_LOW_THRESHOLD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (InternalState::REQUEST_LOW_THRESHOLD): {
      *id = MAX31865::REQUEST_LOW_THRESHOLD;
      return buildCommandFromCommand(*id, nullptr, 0);
    }
    case (InternalState::CLEAR_FAULT_BYTE): {
      *id = MAX31865::CLEAR_FAULT_BYTE;
      return buildCommandFromCommand(*id, nullptr, 0);
    }

    default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "Max31865PT1000Handler: Invalid internal state" << std::endl;
#else
      sif::printError("Max31865PT1000Handler: Invalid internal state\n");
#endif
      return returnvalue::FAILED;
  }
}

ReturnValue_t Max31865PT1000Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                             const uint8_t *commandData,
                                                             size_t commandDataLen) {
  switch (deviceCommand) {
    case (MAX31865::CONFIG_CMD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::CONFIG_CMD);
      if (commandDataLen == 1) {
        commandBuffer[1] = commandData[0];
        currentCfg = commandData[0];
        DeviceHandlerBase::rawPacketLen = 2;
        DeviceHandlerBase::rawPacket = commandBuffer.data();
        return returnvalue::OK;
      } else {
        return DeviceHandlerIF::NO_COMMAND_DATA;
      }
    }
    case (MAX31865::CLEAR_FAULT_BYTE): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::CONFIG_CMD);
      commandBuffer[1] = currentCfg | MAX31865::CLEAR_FAULT_BIT_VAL;
      DeviceHandlerBase::rawPacketLen = 2;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::REQUEST_CONFIG): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::REQUEST_CONFIG);
      commandBuffer[1] = 0x00;  // dummy byte
      DeviceHandlerBase::rawPacketLen = 2;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::WRITE_HIGH_THRESHOLD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::WRITE_HIGH_THRESHOLD);
      commandBuffer[1] = static_cast<uint8_t>(HIGH_THRESHOLD >> 8);
      commandBuffer[2] = static_cast<uint8_t>(HIGH_THRESHOLD & 0xFF);
      DeviceHandlerBase::rawPacketLen = 3;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::REQUEST_HIGH_THRESHOLD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::REQUEST_HIGH_THRESHOLD);
      commandBuffer[1] = 0x00;  // dummy byte
      commandBuffer[2] = 0x00;  // dummy byte
      DeviceHandlerBase::rawPacketLen = 3;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::WRITE_LOW_THRESHOLD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::WRITE_LOW_THRESHOLD);
      commandBuffer[1] = static_cast<uint8_t>(LOW_THRESHOLD >> 8);
      commandBuffer[2] = static_cast<uint8_t>(LOW_THRESHOLD & 0xFF);
      DeviceHandlerBase::rawPacketLen = 3;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::REQUEST_LOW_THRESHOLD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::REQUEST_LOW_THRESHOLD);
      commandBuffer[1] = 0x00;  // dummy byte
      commandBuffer[2] = 0x00;  // dummy byte
      DeviceHandlerBase::rawPacketLen = 3;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::REQUEST_RTD): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::REQUEST_RTD);
      // two dummy bytes
      commandBuffer[1] = 0x00;
      commandBuffer[2] = 0x00;
      DeviceHandlerBase::rawPacketLen = 3;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    case (MAX31865::REQUEST_FAULT_BYTE): {
      commandBuffer[0] = static_cast<uint8_t>(MAX31865::REQUEST_FAULT_BYTE);
      commandBuffer[1] = 0x00;
      DeviceHandlerBase::rawPacketLen = 2;
      DeviceHandlerBase::rawPacket = commandBuffer.data();
      return returnvalue::OK;
    }
    default:
      // Unknown DeviceCommand
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
}

void Max31865PT1000Handler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(MAX31865::CONFIG_CMD, 3);
  insertInCommandAndReplyMap(MAX31865::REQUEST_CONFIG, 3);
  insertInCommandAndReplyMap(MAX31865::WRITE_LOW_THRESHOLD, 3);
  insertInCommandAndReplyMap(MAX31865::REQUEST_LOW_THRESHOLD, 3);
  insertInCommandAndReplyMap(MAX31865::WRITE_HIGH_THRESHOLD, 3);
  insertInCommandAndReplyMap(MAX31865::REQUEST_HIGH_THRESHOLD, 3);
  insertInCommandAndReplyMap(MAX31865::REQUEST_RTD, 3, &sensorDataset);
  insertInCommandAndReplyMap(MAX31865::REQUEST_FAULT_BYTE, 3);
  insertInCommandAndReplyMap(MAX31865::CLEAR_FAULT_BYTE, 3);
}

ReturnValue_t Max31865PT1000Handler::scanForReply(const uint8_t *start, size_t remainingSize,
                                                  DeviceCommandId_t *foundId, size_t *foundLen) {
  size_t rtdReplySize = 3;
  size_t configReplySize = 2;

  if (remainingSize == rtdReplySize and internalState == InternalState::RUNNING) {
    *foundId = MAX31865::REQUEST_RTD;
    *foundLen = rtdReplySize;
    return returnvalue::OK;
  }

  if (remainingSize == 3) {
    switch (internalState) {
      case (InternalState::CONFIG_HIGH_THRESHOLD): {
        *foundLen = 3;
        *foundId = MAX31865::WRITE_HIGH_THRESHOLD;
        commandExecuted = true;
        return returnvalue::OK;
      }
      case (InternalState::REQUEST_HIGH_THRESHOLD): {
        *foundLen = 3;
        *foundId = MAX31865::REQUEST_HIGH_THRESHOLD;
        return returnvalue::OK;
      }
      case (InternalState::CONFIG_LOW_THRESHOLD): {
        *foundLen = 3;
        *foundId = MAX31865::WRITE_LOW_THRESHOLD;
        commandExecuted = true;
        return returnvalue::OK;
      }
      case (InternalState::REQUEST_LOW_THRESHOLD): {
        *foundLen = 3;
        *foundId = MAX31865::REQUEST_LOW_THRESHOLD;
        return returnvalue::OK;
      }
      default: {
        sif::debug << "Max31865PT1000Handler::scanForReply: Unknown internal state" << std::endl;
        return returnvalue::OK;
      }
    }
  }

  if (remainingSize == configReplySize) {
    if (internalState == InternalState::CONFIGURE) {
      commandExecuted = true;
      *foundLen = configReplySize;
      *foundId = MAX31865::CONFIG_CMD;
    } else if (internalState == InternalState::REQUEST_FAULT_BYTE) {
      *foundId = MAX31865::REQUEST_FAULT_BYTE;
      *foundLen = 2;
      internalState = InternalState::RUNNING;
    } else if (internalState == InternalState::CLEAR_FAULT_BYTE) {
      *foundId = MAX31865::CLEAR_FAULT_BYTE;
      *foundLen = 2;
      if (mode == _MODE_START_UP) {
        commandExecuted = true;
      } else {
        internalState = InternalState::RUNNING;
      }
    } else {
      *foundId = MAX31865::REQUEST_CONFIG;
      *foundLen = configReplySize;
    }
  }

  return returnvalue::OK;
}

ReturnValue_t Max31865PT1000Handler::interpretDeviceReply(DeviceCommandId_t id,
                                                          const uint8_t *packet) {
  switch (id) {
    case (MAX31865::REQUEST_CONFIG): {
      if (packet[1] != DEFAULT_CONFIG) {
        if (warningSwitch) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
          // it propably would be better if we at least try one restart..
          sif::warning << "Max31865PT1000Handler: 0x" << std::hex << this->getObjectId()
                       << ": Invalid configuration reply" << std::endl;
#else
          sif::printWarning("Max31865PT1000Handler: %04x: Invalid configuration reply!\n",
                            this->getObjectId());
#endif
          warningSwitch = false;
        }
        return returnvalue::OK;
      }
      // set to true for invalid configs too for now.
      if (internalState == InternalState::REQUEST_CONFIG) {
        commandExecuted = true;
      } else if (internalState == InternalState::RUNNING) {
        // we should propably generate a telemetry with the config byte
        // as payload here.
      }
      break;
    }
    case (MAX31865::REQUEST_LOW_THRESHOLD): {
      uint16_t readLowThreshold = packet[1] << 8 | packet[2];
      if (readLowThreshold != LOW_THRESHOLD) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
                     << this->getObjectId() << ": Missmatch between "
                     << "written and readback value of low threshold register" << std::endl;
#else
        sif::printWarning(
            "Max31865PT1000Handler::interpretDeviceReply: Missmatch between "
            "written and readback value of low threshold register\n");
#endif
#endif
      }
      commandExecuted = true;
      break;
    }
    case (MAX31865::REQUEST_HIGH_THRESHOLD): {
      uint16_t readHighThreshold = (packet[1] << 8) | packet[2];
      if (readHighThreshold != HIGH_THRESHOLD) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
                     << this->getObjectId() << ": Missmatch between "
                     << "written and readback value of high threshold register" << std::endl;
#else
        sif::printWarning(
            "Max31865PT1000Handler::interpretDeviceReply: Missmatch between "
            "written and readback value of high threshold register\n");
#endif
#endif
      }
      commandExecuted = true;
      break;
    }
    case (MAX31865::REQUEST_RTD): {
      // first bit of LSB reply byte is the fault bit
      bool faultBit = packet[2] & 0b0000'0001;
      if (resetFaultBit) {
        internalState = InternalState::CLEAR_FAULT_BYTE;
        resetFaultBit = false;
      } else if (shouldFaultStatusBeRequested(faultBit)) {
        // Maybe we should attempt to restart it?
        internalState = InternalState::REQUEST_FAULT_BYTE;
        resetFaultBit = true;
      }

      // RTD value consists of last seven bits of the LSB reply byte and
      // the MSB reply byte
      uint16_t adcCode = ((packet[1] << 8) | packet[2]) >> 1;
      // Calculate resistance
      float rtdValue = adcCode * EiveMax31855::RTD_RREF_PT1000 / INT16_MAX;
      // calculate approximation
      float approxTemp = adcCode / 32.0 - 256.0;

      if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
        if (debugDivider->checkAndIncrement()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
          sif::info << "Max31865: " << std::setw(24) << std::left << locString << std::right
                    << " | R[Ohm] " << rtdValue << " Ohms | Approx T[C]: " << approxTemp
                    << std::endl;
#else
          sif::printInfo("Max31685: Measured resistance is %f Ohms\n", rtdValue);
          sif::printInfo("Approximated temperature is %f C\n", approxTemp);
#endif
        }
#endif
      }

      PoolReadGuard pg(&sensorDataset);
      if (pg.getReadResult() != returnvalue::OK) {
        // Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
                     << this->getObjectId() << ": Error reading dataset!" << std::endl;
#else
        sif::printWarning(
            "Max31865PT1000Handler::interpretDeviceReply: "
            "Error reading dataset!\n");
#endif
        return pg.getReadResult();
      }

      if (not sensorDataset.isValid()) {
        sensorDataset.setValidity(true, false);
        sensorDataset.rtdValue.setValid(true);
        sensorDataset.temperatureCelcius.setValid(true);
      }

      sensorDataset.rtdValue = rtdValue;
      sensorDataset.temperatureCelcius = approxTemp;
      break;
    }
    case (MAX31865::REQUEST_FAULT_BYTE): {
      currentFaultStatus = packet[1];
      bool faultStatusChanged = (currentFaultStatus != lastFaultStatus);
      // Spam protection
      if (faultStatusChanged or
          ((currentFaultStatus == lastFaultStatus) and (sameFaultStatusCounter < 3))) {
        // TODO: Think about triggering an event here
#if OBSW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
                     << this->getObjectId() << ": Fault byte is: 0b"
                     << std::bitset<8>(currentFaultStatus) << std::endl;
#else
        sif::printWarning(
            "Max31865PT1000Handler::interpretDeviceReply: Fault byte"
            " is: 0b" BYTE_TO_BINARY_PATTERN "\n",
            BYTE_TO_BINARY(faultByte));
#endif
#endif
        if (faultStatusChanged) {
          sameFaultStatusCounter = 0;
        } else {
          sameFaultStatusCounter++;
        }
      }
      if (faultStatusChanged) {
        lastFaultStatus = currentFaultStatus;
      }

      PoolReadGuard pg(&sensorDataset);
      auto result = pg.getReadResult();
      if (result != returnvalue::OK) {
        // Configuration error
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::warning << "Max31865PT1000Handler::interpretDeviceReply: Object ID: " << std::hex
                     << this->getObjectId() << ": Error reading dataset" << std::endl;
#else
        sif::printWarning("Max31865PT1000Handler::interpretDeviceReply: Error reading dataset\n");
#endif
        return result;
      }
      if (faultStatusChanged) {
        sensorDataset.lastErrorByte.setValid(true);
        sensorDataset.lastErrorByte = lastFaultStatus;
      }
      sensorDataset.errorByte.setValid(true);
      sensorDataset.errorByte = currentFaultStatus;

      if (currentFaultStatus != 0) {
        sensorDataset.temperatureCelcius.setValid(false);
      }
      break;
    }
    default:
      break;
  }
  return returnvalue::OK;
}

uint32_t Max31865PT1000Handler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
  return 5000;
}

ReturnValue_t Max31865PT1000Handler::getSwitches(const uint8_t **switches,
                                                 uint8_t *numberOfSwitches) {
  return DeviceHandlerBase::NO_SWITCH;
}

ReturnValue_t Max31865PT1000Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                             LocalDataPoolManager &poolManager) {
  using namespace MAX31865;
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::RTD_VALUE), new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::TEMPERATURE_C), new PoolEntry<float>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::LAST_FAULT_BYTE),
                           new PoolEntry<uint8_t>({0}));
  localDataPoolMap.emplace(static_cast<lp_id_t>(PoolIds::FAULT_BYTE), new PoolEntry<uint8_t>({0}));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(sensorDataset.getSid(), false, 30.0));
  return returnvalue::OK;
}

void Max31865PT1000Handler::setInstantNormal(bool instantNormal) {
  this->instantNormal = instantNormal;
}

void Max31865PT1000Handler::modeChanged() {
  if (mode == MODE_OFF) {
    lastFaultStatus = 0;
    currentFaultStatus = 0;
    sameFaultStatusCounter = 0;
    internalState = InternalState::NONE;
  }
}

void Max31865PT1000Handler::setDeviceInfo(uint8_t idx, std::string locString_) {
  deviceIdx = idx;
  locString = std::move(locString_);
}

void Max31865PT1000Handler::setDebugMode(bool enable) { this->debugMode = enable; }

bool Max31865PT1000Handler::shouldFaultStatusBeRequested(bool faultBit) {
  if ((sameFaultStatusCounter < 3) and faultBit) {
    return true;
  }
  return false;
}