#include "ImtqPollingTask.h"

#include <fcntl.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>

#include "fsfw/FSFW.h"

ImtqPollingTask::ImtqPollingTask(object_id_t imtqPollingTask) : SystemObject(imtqPollingTask) {
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  ipcLock = MutexFactory::instance()->createMutex();
  bufLock = MutexFactory::instance()->createMutex();
}

ReturnValue_t ImtqPollingTask::performOperation(uint8_t operationCode) {
  while (true) {
    ipcLock->lockMutex();
    state = InternalState::IDLE;
    ipcLock->unlockMutex();
    semaphore->acquire();

    comStatus = returnvalue::OK;
    // Stopwatch watch;
    switch (currentRequest) {
      case imtq::RequestType::MEASURE_NO_ACTUATION: {
        // Measured to take 24 ms for debug and release builds.
        // Stopwatch watch;
        handleMeasureStep();
        break;
      }
      case imtq::RequestType::ACTUATE: {
        handleActuateStep();
        break;
      }
      default: {
        break;
      }
    };
  }
  return returnvalue::OK;
}

void ImtqPollingTask::handleMeasureStep() {
  size_t replyLen = 0;
  uint8_t* replyPtr;
  ImtqRepliesDefault replies(replyBuf.data());
  // If some startup handling is added later, set configured after it was done once.
  replies.setConfigured();

  // Can be used later to verify correct timing (e.g. all data has been read)
  clearReadFlagsDefault(replies);
  auto i2cCmdExecMeasure = [&](imtq::CC::CC cc) {
    ccToReplyPtrMeasure(replies, cc, &replyPtr, replyLen);
    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::MGM_MEASUREMENT_LOW_LEVEL_ERROR);
  };

  cmdLen = 1;
  cmdBuf[0] = imtq::CC::GET_SYSTEM_STATE;
  if (i2cCmdExecMeasure(imtq::CC::GET_SYSTEM_STATE) != returnvalue::OK) {
    return;
  }

  ignoreNextActuateRequest =
      (replies.getSystemState()[2] == static_cast<uint8_t>(imtq::mode::SELF_TEST));
  if (ignoreNextActuateRequest) {
    // Do not command anything until self-test is done.
    return;
  }

  if (specialRequest != imtq::SpecialRequest::NONE) {
    auto executeSelfTest = [&](imtq::selfTest::Axis axis) {
      cmdBuf[0] = imtq::CC::SELF_TEST_CMD;
      cmdBuf[1] = axis;
      return i2cCmdExecMeasure(imtq::CC::SELF_TEST_CMD);
    };
    // If a self-test is already ongoing, ignore the request.
    if (replies.getSystemState()[2] != static_cast<uint8_t>(imtq::mode::SELF_TEST)) {
      switch (specialRequest) {
        case (imtq::SpecialRequest::DO_SELF_TEST_POS_X): {
          executeSelfTest(imtq::selfTest::Axis::X_POSITIVE);
          break;
        }
        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X): {
          executeSelfTest(imtq::selfTest::Axis::X_NEGATIVE);
          break;
        }
        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y): {
          executeSelfTest(imtq::selfTest::Axis::Y_POSITIVE);
          break;
        }
        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y): {
          executeSelfTest(imtq::selfTest::Axis::Y_NEGATIVE);
          break;
        }
        case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z): {
          executeSelfTest(imtq::selfTest::Axis::Z_POSITIVE);
          break;
        }
        case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
          executeSelfTest(imtq::selfTest::Axis::Z_NEGATIVE);
          break;
        }
        case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
          cmdBuf[0] = imtq::CC::GET_SELF_TEST_RESULT;
          i2cCmdExecMeasure(imtq::CC::GET_SELF_TEST_RESULT);
          break;
        }
        default: {
          // Should never happen
          break;
        }
      }
      // We are done. Only request self test or results here.
      return;
    }
  }

  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
  if (i2cCmdExecMeasure(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
    return;
  }
  // Takes a bit of time to take measurements. Subtract a bit because of the delay of previous
  // commands.
  TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);

  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
  if (i2cCmdExecMeasure(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
    return;
  }
  bool mgmMeasurementTooOld = false;
  // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
  // have old data. Which can be really bad for ACS. And everything.
  if ((replyPtr[2] >> 7) == 0) {
    replyPtr[0] = false;
    mgmMeasurementTooOld = true;
  }

  cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
  if (i2cCmdExecMeasure(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
    return;
  }

  cmdBuf[0] = imtq::CC::GET_CAL_MTM_MEASUREMENT;
  if (i2cCmdExecMeasure(imtq::CC::GET_CAL_MTM_MEASUREMENT) != returnvalue::OK) {
    return;
  }
  if(mgmMeasurementTooOld) {
    sif::error << "IMTQ: MGM measurement too old" << std::endl;
  }
  return;
}

void ImtqPollingTask::handleActuateStep() {
  uint8_t* replyPtr = nullptr;
  size_t replyLen = 0;
  // No point when self-test mode is active.
  if (ignoreNextActuateRequest) {
    return;
  }
  ImtqRepliesWithTorque replies(replyBufActuation.data());
  // Can be used later to verify correct timing (e.g. all data has been read)
  clearReadFlagsWithTorque(replies);
  auto i2cCmdExecActuate = [&](imtq::CC::CC cc) {
    ccToReplyPtrActuate(replies, cc, &replyPtr, replyLen);
    return i2cCmdExecDefault(cc, replyPtr, replyLen, imtq::ACTUATE_CMD_LOW_LEVEL_ERROR);
  };
  buildDipoleCommand();
  if (i2cCmdExecActuate(imtq::CC::START_ACTUATION_DIPOLE) != returnvalue::OK) {
    return;
  }

  TaskFactory::delayTask(10);

  cmdLen = 1;
  cmdBuf[0] = imtq::CC::START_MTM_MEASUREMENT;
  if (i2cCmdExecActuate(imtq::CC::START_MTM_MEASUREMENT) != returnvalue::OK) {
    return;
  }

  TaskFactory::delayTask(currentIntegrationTimeMs + MGM_READ_BUFFER_TIME_MS);

  cmdBuf[0] = imtq::CC::GET_RAW_MTM_MEASUREMENT;
  if (i2cCmdExecActuate(imtq::CC::GET_RAW_MTM_MEASUREMENT) != returnvalue::OK) {
    return;
  }
  bool measurementWasTooOld = false;
  // See p.39 of the iMTQ user manual. If the NEW bit of the STAT bitfield is not set, we probably
  // have old data. Which can be really bad for ACS. And everything.
  if ((replyPtr[2] >> 7) == 0) {
    measurementWasTooOld = true;
    replyPtr[0] = false;
  }

  cmdBuf[0] = imtq::CC::GET_ENG_HK_DATA;
  if (i2cCmdExecActuate(imtq::CC::GET_ENG_HK_DATA) != returnvalue::OK) {
    return;
  }

  if(measurementWasTooOld) {
    sif::error << "IMTQ: MGM measurement too old" << std::endl;
  }
  return;
}

ReturnValue_t ImtqPollingTask::initialize() { return returnvalue::OK; }

ReturnValue_t ImtqPollingTask::initializeInterface(CookieIF* cookie) {
  i2cCookie = dynamic_cast<I2cCookie*>(cookie);
  if (i2cCookie == nullptr) {
    sif::error << "ImtqPollingTask::initializeInterface: Invalid I2C cookie" << std::endl;
    return returnvalue::FAILED;
  }
  i2cDev = i2cCookie->getDeviceFile().c_str();
  i2cAddr = i2cCookie->getAddress();
  return returnvalue::OK;
}

ReturnValue_t ImtqPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                           size_t sendLen) {
  const auto* imtqReq = reinterpret_cast<const imtq::Request*>(sendData);
  {
    MutexGuard mg(ipcLock);
    if (imtqReq->request == imtq::RequestType::ACTUATE) {
      std::memcpy(dipoles, imtqReq->dipoles, sizeof(dipoles));
      torqueDuration = imtqReq->torqueDuration;
    }
    currentRequest = imtqReq->request;
    specialRequest = imtqReq->specialRequest;
    if (state != InternalState::IDLE) {
      return returnvalue::FAILED;
    }
    state = InternalState::BUSY;
  }
  semaphore->release();

  return returnvalue::OK;
}

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

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

void ImtqPollingTask::ccToReplyPtrMeasure(ImtqRepliesDefault& replies, imtq::CC::CC cc,
                                          uint8_t** replyBuf, size_t& replyLen) {
  replyLen = imtq::getReplySize(cc);
  switch (cc) {
    case (imtq::CC::CC::GET_ENG_HK_DATA): {
      *replyBuf = replies.engHk;
      break;
    }
    case (imtq::CC::CC::SOFTWARE_RESET): {
      *replyBuf = replies.swReset;
      break;
    }
    case (imtq::CC::CC::GET_SYSTEM_STATE): {
      *replyBuf = replies.systemState;
      break;
    }
    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
      *replyBuf = replies.startMtmMeasurement;
      break;
    }
    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
      *replyBuf = replies.rawMgmMeasurement;
      break;
    }
    case (imtq::CC::CC::GET_CAL_MTM_MEASUREMENT): {
      *replyBuf = replies.calibMgmMeasurement;
      break;
    }
    default: {
      *replyBuf = replies.specialRequestReply;
      break;
    }
  }
}

void ImtqPollingTask::ccToReplyPtrActuate(ImtqRepliesWithTorque& replies, imtq::CC::CC cc,
                                          uint8_t** replyBuf, size_t& replyLen) {
  replyLen = imtq::getReplySize(cc);
  switch (cc) {
    case (imtq::CC::CC::START_ACTUATION_DIPOLE): {
      *replyBuf = replies.dipoleActuation;
      break;
    }
    case (imtq::CC::CC::GET_ENG_HK_DATA): {
      *replyBuf = replies.engHk;
      break;
    }
    case (imtq::CC::CC::START_MTM_MEASUREMENT): {
      *replyBuf = replies.startMtmMeasurement;
      break;
    }
    case (imtq::CC::CC::GET_RAW_MTM_MEASUREMENT): {
      *replyBuf = replies.rawMgmMeasurement;
      break;
    }
    default: {
      *replyBuf = nullptr;
      replyLen = 0;
      break;
    }
  }
}
size_t ImtqPollingTask::getExchangeBufLen(imtq::SpecialRequest specialRequest) {
  size_t baseLen = ImtqRepliesDefault::BASE_LEN;
  switch (specialRequest) {
    case (imtq::SpecialRequest::NONE):
    case (imtq::SpecialRequest::DO_SELF_TEST_POS_X):
    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_X):
    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Y):
    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Y):
    case (imtq::SpecialRequest::DO_SELF_TEST_POS_Z):
    case (imtq::SpecialRequest::DO_SELF_TEST_NEG_Z): {
      break;
    }
    case (imtq::SpecialRequest::GET_SELF_TEST_RESULT): {
      baseLen += imtq::replySize::SELF_TEST_RESULTS;
      break;
    }
  }
  return baseLen;
}

void ImtqPollingTask::buildDipoleCommand() {
  cmdBuf[0] = imtq::CC::CC::START_ACTUATION_DIPOLE;
  uint8_t* serPtr = cmdBuf.data() + 1;
  size_t serLen = 0;
  for (uint8_t idx = 0; idx < 3; idx++) {
    SerializeAdapter::serialize(&dipoles[idx], &serPtr, &serLen, cmdBuf.size(),
                                SerializeIF::Endianness::LITTLE);
  }
  SerializeAdapter::serialize(&torqueDuration, &serPtr, &serLen, cmdBuf.size(),
                              SerializeIF::Endianness::LITTLE);
  // sif::debug << "Dipole X: " << dipoles[0] << std::endl;
  // sif::debug << "Torqeu Dur: " << torqueDuration << std::endl;
  cmdLen = 1 + serLen;
}

ReturnValue_t ImtqPollingTask::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                   size_t* size) {
  imtq::RequestType currentRequest;
  {
    MutexGuard mg(ipcLock);
    currentRequest = this->currentRequest;
  }

  size_t replyLen = 0;
  MutexGuard mg(bufLock);
  if (currentRequest == imtq::RequestType::MEASURE_NO_ACTUATION) {
    replyLen = getExchangeBufLen(specialRequest);
    memcpy(exchangeBuf.data(), replyBuf.data(), replyLen);
  } else if (currentRequest == imtq::RequestType::ACTUATE) {
    replyLen = ImtqRepliesWithTorque::BASE_LEN;
    memcpy(exchangeBuf.data(), replyBufActuation.data(), replyLen);
  } else {
    *size = 0;
  }
  *buffer = exchangeBuf.data();
  *size = replyLen;
  return comStatus;
}

void ImtqPollingTask::clearReadFlagsDefault(ImtqRepliesDefault& replies) {
  replies.calibMgmMeasurement[0] = false;
  replies.rawMgmMeasurement[0] = false;
  replies.systemState[0] = false;
  replies.specialRequestReply[0] = false;
  replies.engHk[0] = false;
}

ReturnValue_t ImtqPollingTask::i2cCmdExecDefault(imtq::CC::CC cc, uint8_t* replyPtr,
                                                 size_t replyLen, ReturnValue_t comErrIfFails) {
  ReturnValue_t res = performI2cFullRequest(replyPtr + 1, replyLen);
  if (res != returnvalue::OK) {
    sif::error << "IMTQ: I2C transaction for command 0x" << std::hex << std::setw(2) << cc
               << " failed" << std::dec << std::endl;
    comStatus = comErrIfFails;
    return returnvalue::FAILED;
  }
  if (replyPtr[1] != cc) {
    sif::warning << "IMTQ: Unexpected CC 0x" << std::hex << std::setw(2)
                 << static_cast<int>(replyPtr[1]) << " for command 0x" << cc << std::dec
                 << std::endl;
    comStatus = comErrIfFails;
    return returnvalue::FAILED;
  }
  replyPtr[0] = true;
  return returnvalue::OK;
}

void ImtqPollingTask::clearReadFlagsWithTorque(ImtqRepliesWithTorque& replies) {
  replies.dipoleActuation[0] = false;
  replies.engHk[0] = false;
  replies.rawMgmMeasurement[0] = false;
  replies.startMtmMeasurement[0] = false;
}

ReturnValue_t ImtqPollingTask::performI2cFullRequest(uint8_t* reply, size_t replyLen) {
  int fd = 0;
  if (cmdLen == 0 or reply == nullptr) {
    return returnvalue::FAILED;
  }

  {
    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
    if (fileHelper.getOpenResult() != returnvalue::OK) {
      return fileHelper.getOpenResult();
    }
    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
                   << strerror(errno) << std::endl;
    }

    int written = write(fd, cmdBuf.data(), cmdLen);
    if (written < 0) {
      sif::error << "IMTQ: Failed to send with error code " << errno
                 << ". Error description: " << strerror(errno) << std::endl;
      return returnvalue::FAILED;
    } else if (static_cast<size_t>(written) != cmdLen) {
      sif::error << "IMTQ: Could not write all bytes" << std::endl;
      return returnvalue::FAILED;
    }
  }
#if FSFW_HAL_I2C_WIRETAPPING == 1
  sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl;
  arrayprinter::print(sendData, sendLen);
#endif

  // wait 1 ms like specified in the datasheet. This is the time the IMTQ needs
  // to prepare a reply.
  usleep(1000);

  {
    UnixFileGuard fileHelper(i2cDev, fd, O_RDWR, "ImtqPollingTask::performI2cFullRequest");
    if (fileHelper.getOpenResult() != returnvalue::OK) {
      return fileHelper.getOpenResult();
    }
    if (ioctl(fd, I2C_SLAVE, i2cAddr) < 0) {
      sif::warning << "Opening IMTQ slave device failed with code " << errno << ": "
                   << strerror(errno) << std::endl;
    }
    MutexGuard mg(bufLock);
    int readLen = read(fd, reply, replyLen);
    if (readLen != static_cast<int>(replyLen)) {
      if (readLen < 0) {
        sif::warning << "IMTQ: Reading failed with error code " << errno << " | " << strerror(errno)
                     << std::endl;
      } else {
        sif::warning << "IMTQ: Read only" << readLen << " from " << replyLen << " bytes"
                     << std::endl;
      }
    }
  }
  if (reply[0] == 0xff or reply[1] == 0xff) {
    sif::warning << "IMTQ: No reply available after 1 millisecond";
    return NO_REPLY_AVAILABLE;
  }
  return returnvalue::OK;
}