qt5-legatus-legacy/Lts/Proxies/WirelessLocationSystem/wls_participant.cpp

/**
  * A single Participant
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2017-01-10
  */

#include "wls_participant.h"
#include "wls_sfm_message.h"
#include "wls_handler.h"
#include "settings.h"
#include <QDateTime>
#include <LtsUtilsCore/dds_vector.h>
#include <LtsUtilsCore/sfm_bytemessage_converter.h>

static constexpr qint64 s_dataCommunicationAliveTimeout = 5 * 1000;
static constexpr int s_maxWlsRetryCount = 3;

namespace WirelessLocationSystem
{
WlsParticipant::WlsParticipant(QObject *parent)
  : QObject(parent)
  , m_wlsTeleTimeoutTimer(new QTimer(this))
  , m_logger(getLogger("WlsParticipant"))
{
  m_wlsTeleTimeoutTimer->setSingleShot(true);
  m_wlsTeleTimeoutTimer->setTimerType(Qt::VeryCoarseTimer);
  m_wlsTeleTimeoutTimer->setInterval(s_dataCommunicationAliveTimeout);
  connect(m_wlsTeleTimeoutTimer, &QTimer::timeout, this, &WlsParticipant::onWlsTeleTimeout);
}

QByteArray WlsParticipant::getPlayerIdStr() const
{
  QByteArray ret;
  if (m_equipmentNumber > 0)
    ret = QByteArray::number(m_equipmentNumber);
  else
    ret = "unknown";
  if (m_tagId.isEmpty())
    ret += "(unknown tag id)";
  else
    ret += "(" + m_tagId + ")";
  return ret;
}

void WlsParticipant::setEquipmentNumber(int newEquipmentNumber)
{
  if (m_equipmentNumber == newEquipmentNumber)
    return;
  m_equipmentNumber = newEquipmentNumber;

  emit dataCommunicationStateChanged(this);
  emit positionChanged(this);
}

void WlsParticipant::setPosition(const RelativePosition &newPosition,
                                 qint64 tsCurrentPos)
{
  if (tsCurrentPos == m_lastSeenPosition ||
      (((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout())  && comparePos(m_relativePosition, newPosition)))// same timestamp or same pos for 5 sec
  {
    DEBUG_LOG(m_logger) << "Check for pos Update";
    DEBUG_LOG(m_logger) << "Is Outdated= " <<((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout());

    if (((tsCurrentPos - m_lastSeenPosition) <= Settings::deadPosTimeout())  && comparePos(m_relativePosition, newPosition))
    {
      DEBUG_LOG(m_logger) << "Same Position or not older than 5000msecs " << tsCurrentPos - m_lastSeenPosition;
      return;
    }
    const qint64 tsNow = QDateTime::currentMSecsSinceEpoch();
    if (tsCurrentPos == m_lastSeenPosition && m_lastValidPosition + 9 * 60 * 1000 > tsNow) // younger than 9 min -> ignore //should never be true as agilion always send with new timestamp
      return; // position did not change

    // did not move in the last 9min, send a new position report to excon
    m_lastValidPosition = tsNow;
    if (isDataCommunicationPossible())
      emit positionChanged(this);

    return;
  }

  const int newWlsZone = WlsParticipant::calcWlsZone(newPosition.wlsAreaId,
                                                     newPosition.wlsBuildingId,
                                                     newPosition.wlsRoomNumber);
  setWlsZone(newWlsZone);

  // to create a new position report on restart / after 9 min timeout
  m_lastSeenPosition = tsCurrentPos;
  m_lastValidPosition = tsCurrentPos;

  m_relativePosition = newPosition;
  m_relativePosition.isValid = true;

  if (isDataCommunicationPossible())
    emit positionChanged(this);
}

void WlsParticipant::checkGpsOnOffTimeoutTimer(qint64 tNow)
{
  // full blown tetra mode -> nothing to do
  if (m_gpsMode == GpsMode::On ||
      m_dataCommunication == DataCommunication::Off)
    return;
  if (m_lastSeenDataCommunication > tNow - s_maxWlsRetryCount * s_dataCommunicationAliveTimeout)
    return;

  DEBUG_LOG(m_logger) << getPlayerIdStr() << ": lastSeenDataCommunication: "
                      << m_lastSeenDataCommunication << ", now: "
                      << (tNow - s_maxWlsRetryCount * s_dataCommunicationAliveTimeout)
                      << " (" << tNow << "), "
                      << m_dataCommunication << ", "
                      << m_gpsMode;

  // no alive from participant -> switch back to
  if (m_dataCommunication == DataCommunication::On)
  {
    m_dataCommunication = DataCommunication::Off;
    emit dataCommunicationStateChanged(this);
  }
  if (m_gpsMode == GpsMode::Off)
  {
    m_gpsMode = GpsMode::On;
    emit gpsOnOffChanged(this);
  }
  m_wlsZone = 0;
}

void WlsParticipant::setLastSeenWlsTelegramTime(qint64 ts)
{
  m_lastSeenDataCommunication = ts;
  // current only allowed when wlsZone > 3
  //  m_relativePosition.dataCommunication = WlsParticipant::DataCommunication::On;
}

void WlsParticipant::setWlsZone(int newWlsZone)
{
  const auto oldGpsMode = m_gpsMode;
  const auto oldDataComm = m_dataCommunication;
  if (newWlsZone >= 0) // < 0 == restart
  {
    if (WlsParticipant::mustSendGpsOff(m_wlsZone, newWlsZone))
      m_gpsMode = GpsMode::Off;
    else if (WlsParticipant::mustSendGpsOn(m_wlsZone, newWlsZone))
      m_gpsMode = GpsMode::On;

    m_dataCommunication = (newWlsZone > 3)
                          ? DataCommunication::On
                          : DataCommunication::Off;
    INFO_LOG(m_logger) << getPlayerIdStr() << ": wlsZone old: " << m_wlsZone << ", new: " << newWlsZone
                       << ", new gps mode: " << m_gpsMode
                       << ", new data comm mode: " << m_dataCommunication
                       << ", " << this;
  }
  else
  {
    INFO_LOG(m_logger) << getPlayerIdStr() << ": Reset zone/data comunication status - sfm was restarted";
    m_gpsMode = GpsMode::On;
    m_dataCommunication = DataCommunication::Unknown;
    m_lastSeenPosition = -1;
  }
  m_wlsZone = newWlsZone;

  if (oldDataComm != m_dataCommunication)
  {
    if (m_dataCommunication == DataCommunication::On)
      m_lastSeenDataCommunication = QDateTime::currentMSecsSinceEpoch();
    emit dataCommunicationStateChanged(this);
  }
  if (oldGpsMode != m_gpsMode)
    emit gpsOnOffChanged(this);
}

bool WlsParticipant::handleSfmRxData(const SfmMsgTetra_rx *sfmTele)
{
  if (!isRecvCounterCorrect(sfmTele->sendCounter()))
  {
    WARNING_LOG(m_logger) << "Got data telegram with counter " << int(sfmTele->sendCounter())
                          << " last one was " << int(m_lastRecvCounter) << " - ignoring";
    return false;
  }

  m_bSfmPayloadReady = false;
  if (sfmTele->isFragmented())
  {
    if (m_sfmRxPayloadFragmentNumber + 1 != sfmTele->fragmentNumber())
    {
      WARNING_LOG(m_logger) << getPlayerIdStr() << ": Missing a fragment (waiting for "
                            << (m_sfmRxPayloadFragmentNumber + 1)
                            << ", got " << sfmTele->fragmentNumber() << ")";
      if (sfmTele->fragmentNumber() != 1)
        return false;
      // assume the last fragmented telegram did not came through, start over
      m_sfmRxPayloadFragmentNumber = 0;
    }
    if (m_sfmRxPayloadFragmentNumber == 0)  // first fragment - clear buffer
      m_sfmRxPayloadData.clear();
    ++m_sfmRxPayloadFragmentNumber;
    m_sfmRxPayloadData += sfmTele->payload();
    m_bSfmPayloadReady = (sfmTele->fragmentCount() == sfmTele->fragmentNumber());

    if (m_bSfmPayloadReady) // last fragment - reset fragment counter
      m_sfmRxPayloadFragmentNumber = 0;
  }
  else
  {
    // not fragmented
    if (m_sfmRxPayloadFragmentNumber != 0)
      WARNING_LOG(m_logger) << getPlayerIdStr()
                            << ": Got unfragmented telegram but currently waiting for next fragment of a fragmented telegram";
    m_sfmRxPayloadFragmentNumber = 0; // assume the last fragmenation did not came through
    m_sfmRxPayloadData = sfmTele->payload();
    m_bSfmPayloadReady = true;
  }

  if (m_bSfmPayloadReady)
    emit wlsBinarySfmDataComplete(this);

  return true;
}

bool WlsParticipant::calcChunkCountAndLen(const QByteArray &data, int &chunkCount, int &chunkLen)
{
  // hex as ascii -> / 2
  // STX
  // msgid (2bytes, current msgIds are all fine - no substitution needed),
  // msg length is in message, we assume it must be subsituted (using 0x02 for checks)
  // fragment byte is correctly calculated
  // checksum byte (2 bytes worst case)
  // ETX
  constexpr int maxBytesPerChunk = (WlsHandler::s_maxLenTransparentData / 2) - (1 + 2 + 2 + 1);

  chunkLen = 0;
  chunkCount = 1;
  bool bChunkCountIsOk = true;
  QByteArray chunk;
  for (int i = chunkCount; i < 16; ++i) // max. 15 chunks
  {
    bChunkCountIsOk = true;
    chunkLen = (data.size() + i - 1) / i;

    for (int j = 0; j < i; ++j)
    {
      const char fragByte = SfmMsgTetra_tx::fragmentByte(i, j + 1);
      chunk.clear();
      chunk += 0x02;  // a send counter which needs to be substituted
      chunk += fragByte;
      chunk += data.mid(j * chunkLen, chunkLen);

      const int payloadLen = LtsUtils::lenMaskedData(chunk);
      if (payloadLen > maxBytesPerChunk)
      {
        bChunkCountIsOk = false;
        break;
      }
    }
    if (bChunkCountIsOk)
    {
      chunkCount = i;
      break;
    }
  }
  return bChunkCountIsOk;
}

void WlsParticipant::sendBinarySfmData(const std::vector<unsigned char> &data)
{
  int chunkLen = 0;
  int chunkCount = 0;

  const QByteArray ba = LtsUtils::toQByteArray(data);
  if (!calcChunkCountAndLen(ba, chunkCount, chunkLen))
  {
    WARNING_LOG(m_logger) << getPlayerIdStr()
                          << ": Internal error calculating chunk size for payload size " << data.size();
    return;
  }

  WlsPayloadData payload;
  payload.fragmentCount = chunkCount;
  payload.fragmentNumber = 0;
  int iPos = 0;
  while (iPos < ba.size())
  {
    ++payload.fragmentNumber;
    payload.payload = ba.mid(iPos, chunkLen);
    m_wlsQueue.enqueue(payload);
    iPos += chunkLen;
  }

  QTimer::singleShot(0, this, &WlsParticipant::checkSendTele);
}

void WlsParticipant::handleWlsAck(const Acknowledge_rx *ack)
{
  // the ack recv counter must match our send counter
  if (ack->recvCounter() != m_nextSendCounter)
  {
    WARNING_LOG(m_logger) << "Got ack for telegram with counter " << int(ack->recvCounter())
                          << " but expected " << int(m_nextSendCounter) << " - ignoring";
    return;
  }
  if (ack->isOk())
  {
    if (m_wlsQueue.isEmpty())
    {
      ERROR_LOG(m_logger) << "got an ack but nothing was sent - something went terribly wrong - resetting send/recv counter";
      m_nextSendCounter = 0;
      m_lastRecvCounter = 0;
      return;
    }
    const WlsPayloadData &h = m_wlsQueue.head();
    if (h.fragmentCount > 1)
    {
      if (h.fragmentCount == h.fragmentNumber)
        m_currentFragmentedTeleQueue.clear(); // sent complete fragmented telegram - can throw away the segments
      else
        m_currentFragmentedTeleQueue.enqueue(h);  // save for possible resend
    }
    m_wlsQueue.dequeue();
    m_wlsTeleTimeoutTimer->stop();
  }
  else
  {
    // if we sent a fragmented telegram then we've to start over from the beginning
    while (!m_currentFragmentedTeleQueue.isEmpty())
      m_wlsQueue.push_front(m_currentFragmentedTeleQueue.dequeue());
  }
  checkSendTele();
}

void WlsParticipant::checkSendTele()
{
  if (m_wlsTeleTimeoutTimer->isActive())
    return;
  sendNextWlsTelegram(false);
}

void WlsParticipant::sendNextWlsTelegram(bool bIsResend)
{
  if (m_wlsQueue.isEmpty())
    return;
  if (m_dataCommunication == WlsParticipant::DataCommunication::Off)
  {
    if (bIsResend)
    {
      // when we try to resend due to timeout but now we know that there is no
      // data communication anymore -> throw away
      m_wlsQueue.clear();
      return;
    }
    WARNING_LOG(m_logger) << getPlayerIdStr()
                          << ": Trying to send a wls telegram but no data communication possible";
    // should not happen so leave the assertion to find out the reasons for it
    Q_ASSERT(false);
    m_wlsQueue.clear();
    return;
  }
  const auto &tele = m_wlsQueue.head();
  SfmMsgTetra_tx tx(getNextSendCounter(bIsResend),
                    tele.fragmentCount,
                    tele.fragmentNumber,
                    tele.payload);
  emit dataForWls(this, tx);
  if (!bIsResend)
    m_wlsTeleTimeoutCounter = 0;
  m_wlsTeleTimeoutTimer->start();
}

void WlsParticipant::onWlsTeleTimeout()
{
  ++m_wlsTeleTimeoutCounter;
  DEBUG_LOG(m_logger) << getPlayerIdStr()
                      << ": Resend WLS connection to participant " << m_equipmentNumber
                      << "(" << m_tagId << ")"
                      << " m_wlsTeleTimeoutCounter =" << m_wlsTeleTimeoutCounter
                      << " s_maxWlsRetryCount=" << s_maxWlsRetryCount;
  if (m_wlsTeleTimeoutCounter == s_maxWlsRetryCount)
  {
    WARNING_LOG(m_logger) << getPlayerIdStr()
                          << ": Lost WLS connection to participant " << m_equipmentNumber
                          << "(" << m_tagId << ")";
    m_lastSeenDataCommunication = -1;
    m_wlsTeleTimeoutCounter = 0;
    m_nextSendCounter = 0;
    m_lastRecvCounter = 0;
    checkGpsOnOffTimeoutTimer(QDateTime::currentMSecsSinceEpoch());
    return;
  }
  sendNextWlsTelegram(true);
}

uint8_t WlsParticipant::getNextSendCounter(bool bIsResend)
{
  if (bIsResend)
    return m_nextSendCounter;
  ++m_nextSendCounter;
  if (m_nextSendCounter == 0)
    ++m_nextSendCounter;
  return m_nextSendCounter;
}

bool WlsParticipant::isRecvCounterCorrect(uint8_t recvCounter)
{
  // same as in tetra - the counter may not be monotonically ascending ...
  bool bRet = ((recvCounter == 0 && m_lastRecvCounter == 0) ||
               recvCounter != m_lastRecvCounter);
  m_lastRecvCounter = recvCounter;
  return bRet;
}

TrainingAreaBase::TetraIcd::WlsParticipantState WlsParticipant::wlsParticipantState() const
{
  TrainingAreaBase::TetraIcd::WlsParticipantState ps;
  ps.equipmentNumber = m_equipmentNumber;
  ps.inBlockedArea = false;
  ps.gpsOn = isGpsMode();
  ps.lastSeen = m_lastSeenDataCommunication;
  ps.tacticalState = -1;
  ps.tagId = QString::fromLatin1(tagId());
  ps.voltage = voltage;

  ps.relativePosition.x = m_relativePosition.x;
  ps.relativePosition.y = m_relativePosition.y;
  ps.relativePosition.z = m_relativePosition.z;

  ps.relativePosition.wlsAreaId = m_relativePosition.wlsAreaId;
  ps.relativePosition.wlsZone = m_wlsZone;
  ps.relativePosition.wlsBuildingId = m_relativePosition.wlsBuildingId;
  ps.relativePosition.wlsRoomNumber = m_relativePosition.wlsRoomNumber;
  ps.relativePosition.playerOrientation = m_relativePosition.playerOrientation;
  ps.relativePosition.weaponOrientation = m_relativePosition.weaponOrientation;
  ps.relativePosition.isValid = m_relativePosition.isValid;

  return ps;
}

std::ostream &operator<<(std::ostream &stream, WlsParticipant::GpsMode gm)
{
  switch (gm)
  {
  case WlsParticipant::GpsMode::On:
    return stream << "GpsMode::On";
  case WlsParticipant::GpsMode::Off:
    return stream << "GpsMode::Off";
  }
  return stream << "Unknown GpsMode (" << std::to_string(int(gm)) << ")";
}

std::ostream &operator<<(std::ostream &stream, WlsParticipant::DataCommunication dc)
{
  switch (dc)
  {
  case WlsParticipant::DataCommunication::Unknown:
    return stream << "DataCommunication::Unknown";
  case WlsParticipant::DataCommunication::On:
    return stream << "DataCommunication::On";
  case WlsParticipant::DataCommunication::Off:
    return stream << "DataCommunication::Off";
  }
  return stream << "Unknown DataCommunication (" << std::to_string(int(dc)) << ")";
}

} // namespace WirelessLocationSystem