#include "StrComHandler.h"

#include <fcntl.h>
#include <fsfw/filesystem/HasFileSystemIF.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <mission/acs/str/strHelpers.h>
#include <unistd.h>

#include <filesystem>
#include <fstream>

#include "OBSWConfig.h"
#include "eive/definitions.h"
#include "fsfw/timemanager/Countdown.h"
#include "mission/utility/Filenaming.h"
#include "mission/utility/ProgressPrinter.h"
#include "mission/utility/Timestamp.h"

extern "C" {
#include <sagitta/client/actionreq.h>
}

using namespace returnvalue;

static constexpr bool PACKET_WIRETAPPING = false;

StrComHandler::StrComHandler(object_id_t objectId) : SystemObject(objectId) {
  lock = MutexFactory::instance()->createMutex();
  semaphore.acquire();
}

StrComHandler::~StrComHandler() {}

ReturnValue_t StrComHandler::initialize() {
#ifdef XIPHOS_Q7S
  sdcMan = SdCardManager::instance();
  if (sdcMan == nullptr) {
    sif::warning << "StrHelper::initialize: Invalid SD Card Manager" << std::endl;
    return returnvalue::FAILED;
  }
#endif
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::performOperation(uint8_t operationCode) {
  ReturnValue_t result = returnvalue::OK;
  while (true) {
    lock->lockMutex();
    state = InternalState::SLEEPING;
    lock->unlockMutex();
    semaphore.acquire();
    switch (state) {
      case InternalState::POLL_ONE_REPLY: {
        // Stopwatch watch;
        replyTimeout.setTimeout(400);
        readOneReply(static_cast<uint32_t>(state));
        {
          MutexGuard mg(lock);
          replyWasReceived = true;
        }
        break;
      }
      case InternalState::UPLOAD_IMAGE: {
        replyTimeout.setTimeout(200);
        resetReplyHandlingState();
        result = performImageUpload();
        if (result == returnvalue::OK) {
          triggerEvent(IMAGE_UPLOAD_SUCCESSFUL);
        } else {
          triggerEvent(IMAGE_UPLOAD_FAILED);
        }
        break;
      }
      case InternalState::DOWNLOAD_IMAGE: {
        replyTimeout.setTimeout(200);
        resetReplyHandlingState();
        result = performImageDownload();
        if (result == returnvalue::OK) {
          triggerEvent(IMAGE_DOWNLOAD_SUCCESSFUL);
        } else {
          triggerEvent(IMAGE_DOWNLOAD_FAILED);
        }
        break;
      }
      case InternalState::FLASH_READ: {
        replyTimeout.setTimeout(200);
        resetReplyHandlingState();
        result = performFlashRead();
        if (result == returnvalue::OK) {
          triggerEvent(FLASH_READ_SUCCESSFUL);
        } else {
          triggerEvent(FLASH_READ_FAILED);
        }
        break;
      }
      case InternalState::FIRMWARE_UPDATE: {
        replyTimeout.setTimeout(2000);
        resetReplyHandlingState();
        result = performFirmwareUpdate();
        if (result == returnvalue::OK) {
          triggerEvent(FIRMWARE_UPDATE_SUCCESSFUL);
        } else {
          triggerEvent(FIRMWARE_UPDATE_FAILED);
        }
        break;
      }
      default:
        sif::debug << "StrHelper::performOperation: Invalid state" << std::endl;
        break;
    }
  }
}

ReturnValue_t StrComHandler::startImageUpload(std::string fullname) {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
  }
#ifdef XIPHOS_Q7S
  ReturnValue_t result = checkPath(fullname);
  if (result != returnvalue::OK) {
    return result;
  }
#endif
  uploadImage.uploadFile = fullname;
  if (not std::filesystem::exists(fullname)) {
    return FILE_NOT_EXISTS;
  }
  {
    MutexGuard mg(lock);
    replyWasReceived = false;
    state = InternalState::UPLOAD_IMAGE;
  }
  semaphore.release();
  terminate = false;
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::startImageDownload(std::string path) {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
  }
#ifdef XIPHOS_Q7S
  ReturnValue_t result = checkPath(path);
  if (result != returnvalue::OK) {
    return result;
  }
#endif
  if (not std::filesystem::exists(path)) {
    return PATH_NOT_EXISTS;
  }
  downloadImage.path = path;
  {
    MutexGuard mg(lock);
    replyWasReceived = false;
    state = InternalState::DOWNLOAD_IMAGE;
  }
  terminate = false;
  semaphore.release();
  return returnvalue::OK;
}

void StrComHandler::stopProcess() { terminate = true; }

void StrComHandler::setDownloadImageName(std::string filename) {
  downloadImage.filename = filename;
}

void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; }

ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
                                                 startracker::FirmwareTarget target) {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
  }
#ifdef XIPHOS_Q7S
  ReturnValue_t result = checkPath(fullname);
  if (result != returnvalue::OK) {
    return result;
  }
#endif
  flashWrite.fullname = fullname;
  if (not std::filesystem::exists(flashWrite.fullname)) {
    return FILE_NOT_EXISTS;
  }
  if (target == startracker::FirmwareTarget::MAIN) {
    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_MAIN);
    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_MAIN);
  } else if (target == startracker::FirmwareTarget::BACKUP) {
    flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_BACKUP);
    flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_BACKUP);
  }
  {
    MutexGuard mg(lock);
    replyWasReceived = false;
    state = InternalState::FIRMWARE_UPDATE;
  }
  semaphore.release();
  terminate = false;
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::startFlashRead(std::string path, uint8_t startRegion,
                                            uint32_t length) {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
  }
#ifdef XIPHOS_Q7S
  ReturnValue_t result = checkPath(path);
  if (result != returnvalue::OK) {
    return result;
  }
#endif
  flashRead.path = path;
  if (not std::filesystem::exists(flashRead.path)) {
    return FILE_NOT_EXISTS;
  }
  flashRead.startRegion = startRegion;
  flashRead.size = length;
  {
    MutexGuard mg(lock);
    replyWasReceived = false;
    state = InternalState::FLASH_READ;
  }
  semaphore.release();
  terminate = false;
  return returnvalue::OK;
}

void StrComHandler::disableTimestamping() { timestamping = false; }

void StrComHandler::enableTimestamping() { timestamping = true; }

ReturnValue_t StrComHandler::performImageDownload() {
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  ReturnValue_t result;
#if OBSW_DEBUG_STARTRACKER == 1
  ProgressPrinter progressPrinter("Image download", ImageDownload::LAST_POSITION);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  struct DownloadActionRequest downloadReq;
  uint32_t size = 0;
  uint32_t retries = 0;
  size_t replySize = 0;
  std::string image = Filenaming::generateAbsoluteFilename(downloadImage.path,
                                                           downloadImage.filename, timestamping);
  std::ofstream file(image, std::ios_base::out);
  if (not std::filesystem::exists(image)) {
    return FILE_CREATION_FAILED;
  }
  downloadReq.position = 0;
  while (downloadReq.position < ImageDownload::LAST_POSITION) {
    if (terminate) {
      file.close();
      return returnvalue::OK;
    }
    arc_pack_download_action_req(&downloadReq, cmdBuf.data(), &size);
    result = sendAndRead(size, downloadReq.position);
    if (result != returnvalue::OK) {
      if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
        serial::flushRxBuf(serialPort);
        retries++;
        continue;
      }
      file.close();
      return result;
    }
    result = checkActionReply(replySize, "downloading image");
    if (result != returnvalue::OK) {
      if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
        serial::flushRxBuf(serialPort);
        retries++;
        continue;
      }
      file.close();
      return result;
    }
    result = checkReplyPosition(downloadReq.position);
    if (result != returnvalue::OK) {
      if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
        serial::flushRxBuf(serialPort);
        retries++;
        continue;
      }
      file.close();
      return result;
    }
    file.write(reinterpret_cast<const char*>(replyPtr + IMAGE_DATA_OFFSET), CHUNK_SIZE);
#if OBSW_DEBUG_STARTRACKER == 1
    progressPrinter.print(downloadReq.position);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
    downloadReq.position++;
    retries = 0;
  }
  file.close();
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::performImageUpload() {
  ReturnValue_t result = returnvalue::OK;
  uint32_t size = 0;
  uint32_t imageSize = 0;
  struct UploadActionRequest uploadReq;
  uploadReq.position = 0;
  size_t writtenBytes = 0;
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  std::memset(&uploadReq.data, 0, sizeof(uploadReq.data));
  if (not std::filesystem::exists(uploadImage.uploadFile)) {
    triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast<uint32_t>(state));
    return returnvalue::FAILED;
  }
  std::ifstream file(uploadImage.uploadFile, std::ifstream::binary);
  if (file.bad()) {
    return HasFileSystemIF::GENERIC_FILE_ERROR;
  }

  // Set position of next character to end of file input stream
  file.seekg(0, file.end);
  // tellg returns position of character in input stream
  imageSize = file.tellg();
#if OBSW_DEBUG_STARTRACKER == 1
  ProgressPrinter progressPrinter("Image upload", imageSize);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  size_t fullChunks = imageSize / SIZE_IMAGE_PART;
  size_t remainder = imageSize % SIZE_IMAGE_PART;
  for (size_t idx = 0; idx < fullChunks; idx++) {
    if (terminate) {
      return returnvalue::OK;
    }
    file.seekg(uploadReq.position * SIZE_IMAGE_PART, file.beg);
    file.read(reinterpret_cast<char*>(uploadReq.data), SIZE_IMAGE_PART);
    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
    result = sendAndRead(size, uploadReq.position);
    if (result != returnvalue::OK) {
      return returnvalue::FAILED;
    }
    result = checkActionReply(replyLen, "sky image upload");
    if (result != returnvalue::OK) {
      return result;
    }
#if OBSW_DEBUG_STARTRACKER == 1
    progressPrinter.print((uploadReq.position + 1) * SIZE_IMAGE_PART);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
    uploadReq.position++;
    writtenBytes += SIZE_IMAGE_PART;

    // This does a bit of delaying roughly every second
    if (uploadReq.position % 50 == 0) {
      // Some grace time for other tasks
      TaskFactory::delayTask(2);
    }
  }
  if (remainder > 0) {
    std::memset(uploadReq.data, 0, sizeof(uploadReq.data));
    file.seekg(fullChunks * SIZE_IMAGE_PART, file.beg);
    file.read(reinterpret_cast<char*>(uploadReq.data), remainder);
    file.close();
    arc_pack_upload_action_req(&uploadReq, cmdBuf.data(), &size);
    result = sendAndRead(size, uploadReq.position);
    if (result != returnvalue::OK) {
      return returnvalue::FAILED;
    }
    result = checkActionReply(replyLen, "sky image upload");
    if (result != returnvalue::OK) {
      return result;
    }
  }
#if OBSW_DEBUG_STARTRACKER == 1
  progressPrinter.print((uploadReq.position + 1) * SIZE_IMAGE_PART);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::performFirmwareUpdate() {
  using namespace startracker;
  ReturnValue_t result = returnvalue::OK;
  result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion);
  if (result != returnvalue::OK) {
    return result;
  }
  result = performFlashWrite();
  return result;
}

ReturnValue_t StrComHandler::performFlashWrite() {
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  ReturnValue_t result = returnvalue::OK;
  uint32_t size = 0;
  uint32_t bytesWrittenInRegion = 0;
  size_t totalBytesWritten = 0;
  uint32_t fileSize = 0;

  struct WriteActionRequest req;
  if (not std::filesystem::exists(flashWrite.fullname)) {
    triggerEvent(STR_HELPER_FILE_NOT_EXISTS, static_cast<uint32_t>(state));
    return returnvalue::FAILED;
  }
  std::ifstream file(flashWrite.fullname, std::ifstream::binary);
  if (file.bad()) {
    return returnvalue::FAILED;
  }
  file.seekg(0, file.end);
  fileSize = file.tellg();
  if (fileSize > FLASH_REGION_SIZE * (flashWrite.lastRegion - flashWrite.firstRegion)) {
    sif::debug << "Last region: " << (int)flashWrite.lastRegion
               << " first region: " << (int)flashWrite.firstRegion << std::endl;
    sif::warning << "StrHelper::performFlashWrite: Invalid file" << std::endl;
    return returnvalue::FAILED;
  }
#if OBSW_DEBUG_STARTRACKER == 1
  ProgressPrinter progressPrinter("Flash write", fileSize);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  uint32_t fileChunks = fileSize / CHUNK_SIZE;
  bytesWrittenInRegion = 0;
  req.region = flashWrite.firstRegion;
  req.length = CHUNK_SIZE;

  auto writeNextSegment = [&](uint32_t chunkIdx) {
    file.seekg(chunkIdx * CHUNK_SIZE, file.beg);
    file.read(reinterpret_cast<char*>(req.data), CHUNK_SIZE);
    if (bytesWrittenInRegion + CHUNK_SIZE > FLASH_REGION_SIZE) {
      req.region++;
      bytesWrittenInRegion = 0;
    }
    req.address = bytesWrittenInRegion;
    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
    result = sendAndRead(size, req.address);
    if (result != returnvalue::OK) {
      return result;
    }
    result = checkActionReply(replyLen, "firmware image upload");
    if (result != returnvalue::OK) {
      return result;
    }
    totalBytesWritten += CHUNK_SIZE;
    bytesWrittenInRegion += CHUNK_SIZE;
#if OBSW_DEBUG_STARTRACKER == 1
    progressPrinter.print(chunkIdx * CHUNK_SIZE);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
    return result;
  };

  for (uint32_t idx = 0; idx < fileChunks; idx++) {
    if (terminate) {
      return returnvalue::OK;
    }
    result = writeNextSegment(idx);
    if (result != returnvalue::OK) {
      return result;
    }
    if (idx % 50 == 0) {
      // Some grace time for other tasks
      TaskFactory::delayTask(2);
    }
  }
  uint32_t remainingBytes = fileSize - fileChunks * CHUNK_SIZE;
  if (remainingBytes > 0) {
    file.seekg(fileChunks * CHUNK_SIZE, file.beg);
    file.read(reinterpret_cast<char*>(req.data), remainingBytes);
    file.close();
    if (bytesWrittenInRegion + CHUNK_SIZE > FLASH_REGION_SIZE) {
      req.region++;
      bytesWrittenInRegion = 0;
    }
    req.address = bytesWrittenInRegion;
    req.length = remainingBytes;
    totalBytesWritten += CHUNK_SIZE;
    bytesWrittenInRegion += remainingBytes;
    arc_pack_write_action_req(&req, cmdBuf.data(), &size);
    result = sendAndRead(size, req.address);
    if (result != returnvalue::OK) {
      return result;
    }
    result = checkActionReply(replyLen, "flash write");
    if (result != returnvalue::OK) {
      return result;
    }
  }
#if OBSW_DEBUG_STARTRACKER == 1
  progressPrinter.print(fileSize);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::performFlashRead() {
#ifdef XIPHOS_Q7S
  if (not sdcMan->getActiveSdCard()) {
    return HasFileSystemIF::FILESYSTEM_INACTIVE;
  }
#endif
  ReturnValue_t result;
#if OBSW_DEBUG_STARTRACKER == 1
  ProgressPrinter progressPrinter("Flash read", flashRead.size);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  struct ReadActionRequest req;
  uint32_t bytesRead = 0;
  uint32_t size = 0;
  uint32_t retries = 0;
  Timestamp timestamp;
  std::string fullname =
      Filenaming::generateAbsoluteFilename(flashRead.path, flashRead.filename, timestamping);
  std::ofstream file(fullname, std::ios_base::app | std::ios_base::out);
  if (not std::filesystem::exists(fullname)) {
    return FILE_CREATION_FAILED;
  }
  req.region = flashRead.startRegion;
  req.address = 0;
  while (bytesRead < flashRead.size) {
    if (terminate) {
      return returnvalue::OK;
    }
    if ((flashRead.size - bytesRead) < CHUNK_SIZE) {
      req.length = flashRead.size - bytesRead;
    } else {
      req.length = CHUNK_SIZE;
    }
    arc_pack_read_action_req(&req, cmdBuf.data(), &size);
    result = sendAndRead(size, req.address);
    if (result != returnvalue::OK) {
      if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
        serial::flushRxBuf(serialPort);
        retries++;
        continue;
      }
      file.close();
      return result;
    }
    result = checkActionReply(replyLen, "flash read");
    if (result != returnvalue::OK) {
      if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
        serial::flushRxBuf(serialPort);
        retries++;
        continue;
      }
      file.close();
      return result;
    }
    file.write(reinterpret_cast<const char*>(replyPtr + FLASH_READ_DATA_OFFSET), req.length);
    bytesRead += req.length;
    req.address += req.length;
    if (req.address >= FLASH_REGION_SIZE) {
      req.address = 0;
      req.region++;
    }
    retries = 0;
#if OBSW_DEBUG_STARTRACKER == 1
    progressPrinter.print(bytesRead);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  }
  file.close();
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) {
  ReturnValue_t result = returnvalue::OK;

  const uint8_t* sendData;
  size_t txFrameLen = 0;
  datalinkLayer.encodeFrame(cmdBuf.data(), size, &sendData, txFrameLen);
  int writeResult = write(serialPort, sendData, txFrameLen);
  if (writeResult < 0) {
    sif::warning << "StrHelper::sendAndRead: Failed to send packet" << std::endl;
    triggerEvent(STR_HELPER_SENDING_PACKET_FAILED, result, failParameter);
    return returnvalue::FAILED;
  }

  return readOneReply(failParameter);
}

ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) {
  uint8_t type = startracker::getReplyFrameType(replyPtr);
  if (type != TMTC_ACTIONREPLY) {
    sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl;
    return INVALID_TYPE_ID;
  }
  uint8_t status = startracker::getStatusField(replyPtr);
  if (status != ArcsecDatalinkLayer::STATUS_OK) {
    sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": "
                 << static_cast<unsigned int>(status) << std::endl;
    return STATUS_ERROR;
  }
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::checkReplyPosition(uint32_t expectedPosition) {
  uint32_t receivedPosition = 0;
  std::memcpy(&receivedPosition, replyPtr + POS_OFFSET, sizeof(receivedPosition));
  if (receivedPosition != expectedPosition) {
    triggerEvent(POSITION_MISMATCH, receivedPosition);
    return returnvalue::FAILED;
  }
  return returnvalue::OK;
}

#ifdef XIPHOS_Q7S
ReturnValue_t StrComHandler::checkPath(std::string name) {
  if (name.substr(0, sizeof(config::SD_0_MOUNT_POINT)) == std::string(config::SD_0_MOUNT_POINT)) {
    if (!sdcMan->isSdCardUsable(sd::SLOT_0)) {
      sif::warning << "StrHelper::checkPath: SD card 0 not mounted" << std::endl;
      return SD_NOT_MOUNTED;
    }
  } else if (name.substr(0, sizeof(config::SD_1_MOUNT_POINT)) ==
             std::string(config::SD_1_MOUNT_POINT)) {
    if (!sdcMan->isSdCardUsable(sd::SLOT_0)) {
      sif::warning << "StrHelper::checkPath: SD card 1 not mounted" << std::endl;
      return SD_NOT_MOUNTED;
    }
  }
  return returnvalue::OK;
}
#endif

ReturnValue_t StrComHandler::initializeInterface(CookieIF* cookie) {
  if (cookie == nullptr) {
    return returnvalue::FAILED;
  }
  SerialCookie* serCookie = dynamic_cast<SerialCookie*>(cookie);
  if (serCookie == nullptr) {
    return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
  }
  // comCookie = serCookie;
  std::string devname = serCookie->getDeviceFile();
  /* Get file descriptor */
  serialPort = open(devname.c_str(), O_RDWR);
  if (serialPort < 0) {
    sif::warning << "StrComHandler: open call failed with error [" << errno << ", "
                 << strerror(errno) << std::endl;
    return returnvalue::FAILED;
  }
  // Setting up UART parameters
  tty.c_cflag &= ~PARENB;  // Clear parity bit
  serial::setStopbits(tty, serCookie->getStopBits());
  serial::setBitsPerWord(tty, BitsPerWord::BITS_8);
  tty.c_cflag &= ~CRTSCTS;  // Disable RTS/CTS hardware flow control
  serial::enableRead(tty);
  serial::ignoreCtrlLines(tty);

  // Use non-canonical mode and clear echo flag
  tty.c_lflag &= ~(ICANON | ECHO);

  // Non-blocking mode, use polling
  tty.c_cc[VTIME] = 0;
  tty.c_cc[VMIN] = 0;

  serial::setBaudrate(tty, serCookie->getBaudrate());
  if (tcsetattr(serialPort, TCSANOW, &tty) != 0) {
    sif::warning << "ScexUartReader::initializeInterface: tcsetattr call failed with error ["
                 << errno << ", " << strerror(errno) << std::endl;
  }
  // Flush received and unread data
  tcflush(serialPort, TCIOFLUSH);
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                         size_t sendLen) {
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
  }
  // Ensure consistent state.
  resetReplyHandlingState();

  const uint8_t* txFrame;
  size_t frameLen;
  datalinkLayer.encodeFrame(sendData, sendLen, &txFrame, frameLen);
  if (PACKET_WIRETAPPING) {
    sif::debug << "Sending STR frame" << std::endl;
    arrayprinter::print(txFrame, frameLen);
  }
  ssize_t bytesWritten = write(serialPort, txFrame, frameLen);
  if (bytesWritten != static_cast<ssize_t>(frameLen)) {
    sif::warning << "StrComHandler: Sending packet failed" << std::endl;
    return returnvalue::FAILED;
  }
  // Hacky, but the alternatives look bleak. The raw data contains the information we need
  // and there are not too many special cases.
  if (sendData[0] == TMTC_ACTIONREQ) {
    // 1 is a firmware boot request and 7 is a reboot request. For both, no reply is expected.
    if (sendData[1] == 7 or sendData[1] == 1) {
      return returnvalue::OK;
    }
  }
  {
    MutexGuard mg(lock);
    state = InternalState::POLL_ONE_REPLY;
  }
  // Unlock task to perform reply reading.
  semaphore.release();
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }

ReturnValue_t StrComHandler::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
  return returnvalue::OK;
}

ReturnValue_t StrComHandler::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
  bool replyWasReceived = false;
  {
    MutexGuard mg(lock);
    if (state != InternalState::SLEEPING) {
      return BUSY;
    }
    replyWasReceived = this->replyWasReceived;
  }
  if (not replyWasReceived) {
    *size = 0;
    return returnvalue::OK;
  }
  if (replyResult == returnvalue::OK) {
    *buffer = const_cast<uint8_t*>(replyPtr);
    *size = replyLen;
  }
  replyLen = 0;
  return replyResult;
}

ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
  ReturnValue_t result = returnvalue::OK;
#if OBSW_DEBUG_STARTRACKER == 1
  ProgressPrinter progressPrinter("Unlock and erase", to - from);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  struct UnlockActionRequest unlockReq;
  struct EraseActionRequest eraseReq;
  uint32_t size = 0;
  for (uint32_t idx = from; idx < to; idx++) {
    unlockReq.region = idx;
    unlockReq.code = startracker::region_secrets::SECRETS[idx];
    arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
    result = sendAndRead(size, unlockReq.region);
    if (result != returnvalue::OK) {
      return result;
    }
    result = checkActionReply(replyLen, "unlocking region");
    if (result != returnvalue::OK) {
      sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id "
                   << static_cast<unsigned int>(unlockReq.region) << std::endl;
      return result;
    }
    eraseReq.region = idx;
    arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
    result = sendAndRead(size, eraseReq.region);
    if (result != returnvalue::OK) {
    }
    result = checkActionReply(replyLen, "erasing region");
    if (result != returnvalue::OK) {
      sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id "
                   << static_cast<unsigned int>(eraseReq.region) << std::endl;
      return result;
    }
#if OBSW_DEBUG_STARTRACKER == 1
    progressPrinter.print(idx - from);
#endif /* OBSW_DEBUG_STARTRACKER == 1 */
  }
  return result;
}

ReturnValue_t StrComHandler::handleSerialReception() {
  ssize_t bytesRead = read(serialPort, reinterpret_cast<void*>(recBuf.data()),
                           static_cast<unsigned int>(recBuf.size()));
  if (bytesRead == 0) {
    return NO_SERIAL_DATA_READ;
  } else if (bytesRead < 0) {
    sif::warning << "StrComHandler: read call failed with error [" << errno << ", "
                 << strerror(errno) << "]" << std::endl;
    return FAILED;
  } else if (bytesRead >= static_cast<int>(recBuf.size())) {
    sif::error << "StrComHandler: Receive buffer too small for " << bytesRead << " bytes"
               << std::endl;
    return FAILED;
  } else if (bytesRead > 0) {
    if (PACKET_WIRETAPPING) {
      sif::info << "Received " << bytesRead << " bytes from the STR" << std::endl;
      arrayprinter::print(recBuf.data(), bytesRead);
    }
    datalinkLayer.feedData(recBuf.data(), bytesRead);
  }
  return OK;
}

ReturnValue_t StrComHandler::readOneReply(uint32_t failParameter) {
  ReturnValue_t result;
  uint32_t nextDelayMs = 1;
  replyTimeout.resetTimer();
  while (true) {
    handleSerialReception();
    result = datalinkLayer.checkRingBufForFrame(&replyPtr, replyLen);
    if (result == returnvalue::OK) {
      if (PACKET_WIRETAPPING) {
        sif::debug << "Received STR reply frame" << std::endl;
        arrayprinter::print(replyPtr, replyLen);
      }
      return returnvalue::OK;
    } else if (result != ArcsecDatalinkLayer::DEC_IN_PROGRESS) {
      triggerEvent(STR_HELPER_DEC_ERROR, result, failParameter);
      return DECODING_ERROR;
    }
    if (replyTimeout.hasTimedOut()) {
      triggerEvent(STR_COM_REPLY_TIMEOUT, failParameter, replyTimeout.getTimeoutMs());
      return RECEPTION_TIMEOUT;
    }
    TaskFactory::delayTask(nextDelayMs);
    if (nextDelayMs < 32) {
      nextDelayMs *= 2;
    }
  }
}

void StrComHandler::resetReplyHandlingState() {
  serial::flushRxBuf(serialPort);
  datalinkLayer.reset();
}