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

/**
  * Convert from/to WLS protocol / handler incoming WLS connection
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2017-01-10
  */


#include "wls_handler.h"

#include "main_handler.h"
#include "settings.h"
#include "wls_sfm_message_converter.h"
#include "web_socket_handler.h"

#include <QTcpSocket>
#include <QDateTime>
#include <web_socket_handler.h>

namespace WirelessLocationSystem
{

static constexpr unsigned char s_startMark = 0x7F;
static constexpr unsigned char s_endMark = 0x8F;
static inline QByteArray escapeData(const QByteArray &bufferData)
{
  QByteArray ret;
  ret.reserve(bufferData.size() * 6 / 5);
  for (const unsigned char c : bufferData)
  {
    if (c >= 0x20 && c < 0x7f)
      ret += c;
    else
      ret += "\\x" + QByteArray::number(static_cast<unsigned char>(c), 16);
  }
  return ret;
}

WlsHandler::WlsHandler(QObject *parent)
  : QObject(parent)
  , m_logger(getLogger("WlsHandler"))
{
  if (Settings::wlsMgrsBasePoint().size() <= 15)
    WARNING_LOG(m_logger) << "WLS basepoint is not defined precise enough: " << Settings::wlsMgrsBasePoint();
}

WlsHandler::~WlsHandler()
{
  delete m_tcpServer;
}

bool WlsHandler::init(WlsParticipantHandler *participantHandler,
                      WebSocketHandler *wsHandler,
                      quint16 listenPort)
{
  m_participantHandler = participantHandler;
  m_wsHandler = wsHandler;
  INFO_LOG(m_logger) << "WlsHandler::init";
  delete m_tcpServer;
  m_tcpServer = new QTcpServer(this);
  connect(m_tcpServer, &QTcpServer::newConnection,
          this, &WlsHandler::onIncomingConnection);
  if (!m_tcpServer->listen(QHostAddress::Any, listenPort))
  {
    WARNING_LOG(m_logger) << "Error listening on port " << listenPort;
    return false;
  }

  m_wlsZOffset = Settings::wlsZOffset();

  return true;
}

bool WlsHandler::sendWlsMessage(const WlsParticipant *p, const SfmMessage_tx &msg)
{
  return sendWlsMessage(p->tagId(), msg);
}

bool WlsHandler::sendWlsMessage(const QByteArray &tagId, const SfmMessage_tx &msg)
{
  return sendWlsMessage(tagId, WlsTelegramId::PayloadHostToMobile, SfmMessageConverter::generateTxMessage(msg));
}

bool WlsHandler::sendWlsMessage(const QByteArray &tagId, WlsTelegramId telId, const QByteArray &payload)
{
  if (!isConnected())
    return false;

  if (telId != WlsTelegramId::PayloadHostToMobile)
    return false;

  auto itSocket = m_tcpSocketByTagId.constFind(tagId);
  if (itSocket == m_tcpSocketByTagId.constEnd())
  {
    ERROR_LOG(m_logger) << "Can't find a TCP-socket for tag id: " << tagId;
    return false;
  }

  const unsigned int fragmentParts = ((payload.size() + s_maxLenTransparentData - 1) / s_maxLenTransparentData);
  const QDateTime dt = QDateTime::currentDateTimeUtc();
  QByteArray data;
  // start/end mark, 3*';', telId (can be 2 digits), timestring, \0
  data.reserve(2 + 3 + 2 + 23 + tagId.size() + s_maxLenTransparentData + 1);

  for (unsigned int i = 0; i < fragmentParts; i++)
  {
    data.clear();

    data.append(s_startMark);                                           // Startkennung
    data.append(QByteArray::number(static_cast<int>(telId)));
    data.append(';');                                                   // Trennzeichen
    data.append(dt.toString(QStringLiteral("yyyy-MM-dd hh:mm:ss.zzz")).toLatin1());     // Aktuelle Zeit
    data.append(';');                                                   // Trennzeichen
    data.append(tagId);                                                 // Tag-ID
    data.append(';');                                                   // Trennzeichen
    data.append(payload.mid(i * s_maxLenTransparentData, s_maxLenTransparentData));
    data.append(s_endMark);                                             // Endekennung

    (*itSocket)->write(data);
    DEBUG_LOG(m_logger) << "Sending telegram to wls: " << escapeData(data);
  }
  (*itSocket)->flush();
  return true;
}

void WlsHandler::onIncomingConnection()
{
  INFO_LOG(m_logger) << "WlsHandler::onIncomingConnection";
  QTcpSocket *tcpSocket = m_tcpServer->nextPendingConnection();
  if (!tcpSocket)
    return;
  m_tcpSockets[tcpSocket].clear();

  connect(tcpSocket, &QTcpSocket::readyRead,
          this, &WlsHandler::onIncomingData);
  connect(tcpSocket, &QTcpSocket::disconnected,
          this, &WlsHandler::onSocketDisconnected);

  // send wls connection state to all web sockets
  TrainingAreaBase::TetraIcd::ProxyConnectionState connState;
  connState.connectedToSystem = true;
  m_wsHandler->sendTelegram(&connState, 0);
}

void WlsHandler::onIncomingData()
{

  QTcpSocket *s = qobject_cast<QTcpSocket *>(sender());
  if (!s)
    return;
  auto &tcpBuffer = m_tcpSockets[s];
  const QByteArray bufferData = tcpBuffer + s->readAll();
  TRACE_LOG(m_logger) << "WlsHandler::onIncomingData bufferData='" << escapeData(bufferData) << "'";
  // returns the unparsed part of the buffer
  tcpBuffer = parseBuffer(bufferData, s);

}

void WlsHandler::onSocketDisconnected()
{
  QTcpSocket *s = qobject_cast<QTcpSocket *>(sender());
  if (!s)
    return;
  INFO_LOG(m_logger) << "Socket disconnected";
  m_tcpSockets.remove(s);

  // remove tag ID to socket mappings
  auto it = m_tcpSocketByTagId.begin();
  while (it != m_tcpSocketByTagId.end())
    if (it.value() == s)
      it = m_tcpSocketByTagId.erase(it);
    else
      it++;

  s->deleteLater();

  // send wls connection state to all web sockets
  TrainingAreaBase::TetraIcd::ProxyConnectionState connState;
  connState.connectedToSystem = !m_tcpSockets.isEmpty();
  m_wsHandler->sendTelegram(&connState, 0);
}

QByteArray WlsHandler::parseBuffer(const QByteArray &buffer, QTcpSocket *socket)
{
  int iEndPos = 0;
  int iCurReadPos = 0;
  while (findNextSyncStartByte(buffer, iCurReadPos))
  {
    const int iStartPos = iCurReadPos;
    if (!findNextSyncEndByte(buffer, iCurReadPos))
      break;
    iEndPos = iCurReadPos;
    parseWlsTelegram(buffer.mid(iStartPos + 1, iEndPos - iStartPos - 1), socket);
  }
  return buffer.mid(iEndPos);
}

void WlsHandler::parseWlsTelegram(const QByteArray &baTele, QTcpSocket *socket)
{
  const QList<QByteArray> parts = baTele.split(';');
  if (parts.size() < 3)
    return;

  // insert/update socket for tagId
  m_tcpSocketByTagId.insert(parts.at(2), socket);

  const WlsTelegramId telegramId = static_cast<WlsTelegramId>(parts.at(0).toInt());
  const QDateTime timestamp = QDateTime::fromString(QString::fromUtf8(parts.at(1)), QStringLiteral("yyyy-MM-dd hh:mm:ss.zzz"));
  if (telegramId != WlsTelegramId::PlayerPosition)
    DEBUG_LOG(m_logger) << "Handling " << telegramId
                        << ", data: '" << escapeData(baTele) << "'";
  bool bOk = true;
  switch (telegramId)
  {
  case WlsTelegramId::VersionInformation:
    bOk = handleVersionsinformation(parts);
    break;
  case WlsTelegramId::PlayerPosition:
    bOk = handlePlayerPosition(parts, timestamp);
    break;
  case WlsTelegramId::PlayerState:
    bOk = handlePlayerState(parts);
    break;
  case WlsTelegramId::PayloadMobileToHost:
    bOk = handleDataPayloadMobileToHost(parts);
    break;
  case WlsTelegramId::PayloadHostToMobile:
    WARNING_LOG(m_logger) << "Protocol error - got a message which should not be sent from Wls (PayloadHostToMobile)";
    break;
  }
  if (!bOk)
    WARNING_LOG(m_logger) << "Error handling telegram: '" << baTele << "'";
}

bool WlsHandler::findNextSyncStartByte(const QByteArray &buffer, int &iCurReadPos)
{
  // Suchen nach s_startMark
  while (iCurReadPos < buffer.size())
  {
    const unsigned char data = buffer.at(iCurReadPos);
    if (data == s_startMark)
      return true;
    ++iCurReadPos;
  }
  return false;
}

bool WlsHandler::findNextSyncEndByte(const QByteArray &buffer, int &iCurReadPos)
{
  // Suchen nach s_endMark
  while (iCurReadPos < buffer.size())
  {
    const unsigned char data = buffer.at(iCurReadPos);
    if (data == s_endMark)
      return true;
    ++iCurReadPos;
  }
  return false;
}

bool WlsHandler::handleVersionsinformation(const QList<QByteArray> &data)
{
  if (data.size() < 4)
    return false;
  DEBUG_LOG(m_logger) << "TX(WLS): Version = " << data.at(3);
  return true;
}

bool WlsHandler::handlePlayerPosition(const QList<QByteArray> &data, const QDateTime &timestamp)
{
  if (data.size() < 6)
    return false;

  WlsParticipant *p = participantFromTagId(data.at(2));

  WlsParticipant::RelativePosition pos;
  int iPos = 3;
  if (!parseDoubleCm(data.at(iPos++), pos.x))
    return false;
  if (!parseDoubleCm(data.at(iPos++), pos.y))
    return false;
  if (!parseDoubleCm(data.at(iPos++), pos.z))
    return false;
  if ((data.size() > iPos) && !parseInteger(data.at(iPos++), pos.wlsAreaId))
    return false;
  if ((data.size() > iPos) && !parseInteger(data.at(iPos++), pos.wlsBuildingId))
    return false;
  if ((data.size() > iPos) && !parseInteger(data.at(iPos++), pos.wlsRoomNumber))
    return false;
  if ((data.size() > iPos) && !parseInteger(data.at(iPos++), pos.playerOrientation))
    return false;
  if ((data.size() > iPos) && !parseInteger(data.at(iPos++), pos.weaponOrientation))
    return false;

  pos.z += m_wlsZOffset;
  p->setPosition(pos, timestamp.toMSecsSinceEpoch());

  INFO_LOG(m_logger) << p->getPlayerIdStr() << ": handlePlayerPosition"
                     << " x=" << pos.x << ", y=" << pos.y << ", z=" << pos.z
                     << ", wlsAreaId=" << pos.wlsAreaId
                     << ", wlsBuildingId=" << pos.wlsBuildingId
                     << ", wlsRoomNumber=" << pos.wlsRoomNumber
                     << ", zone=" << p->wlsZone()
                     << ", gpsMode=" << p->gpsMode()
                     << ", dataCommunicationPossible: " << p->dataCommunication()
                     << ", " << timestamp.toString();

  return true;
}

bool WlsHandler::handlePlayerState(const QList<QByteArray> &data)
{
  if (data.size() < 6)
    return false;

  WlsParticipant *p = participantFromTagId(data.at(2));

  int timeout;
  if (!parseInteger(data.at(3), timeout))
    return false;

  int tacticalState;
  if (!parseInteger(data.at(4), tacticalState))
    return false;

  int voltage;
  if (!parseInteger(data.at(5), voltage))
    return false;

  p->voltage = voltage / 10.; // 1/10V

  INFO_LOG(m_logger) << p->getPlayerIdStr() << ": handlePlayerState"
                     << " timeout=" << timeout
                     << " ,tacticalState=" << tacticalState
                     << " ,voltage=" << p->voltage;

  return true;
}

bool WlsHandler::checkPayloadIsHex(const QByteArray &payload) const
{
  for (const char c : payload)
  {
    if ((c < '0' || c > '9') &&
        (c < 'a' || c > 'f') &&
        (c < 'A' || c > 'F'))
    {
      WARNING_LOG(m_logger) << "Payload data is not hex coded ...";
      return false;
    }
  }
  return true;
}

bool WlsHandler::handleDataPayloadMobileToHost(const QList<QByteArray> &data)
{
  if (data.size() < 4)
    return false;

  const QByteArray &payload = data.at(3);
  const QByteArray &tagId = data.at(2);
  if (!checkPayloadIsHex(payload))
    return false;

  QSharedPointer<SfmMessage_rx> msg = SfmMessageConverter::generateRxMessage(payload);
  if (!msg)
  {
    WARNING_LOG(m_logger) << "Can not convert payload to SfmMessage ... ";
    return false;
  }

  WlsParticipant *p = participantFromTagId(tagId, msg->getId() != SfmMessage::MsgId::EquipmentId);
  p->setLastSeenWlsTelegramTime(QDateTime::currentMSecsSinceEpoch());

  switch (msg->getId())
  {
  case SfmMessage::MsgId::EquipmentId:
    handlePayload(p, static_cast<SfmMsgEquipmentId *>(msg.data()));
    break;
  case SfmMessage::MsgId::RequestGpsControl:
    handlePayload(p, static_cast<SfmMsgRequestGpsControl_rx *>(msg.data()));
    break;
  case SfmMessage::MsgId::SfmRestartNotification:
    handlePayload(p, static_cast<SfmMsgSfmRestart_rx *>(msg.data()));
    break;
  case SfmMessage::MsgId::Acknowledge:
    handlePayload(p, static_cast<Acknowledge_rx *>(msg.data()));
    break;
  case SfmMessage::MsgId::TetraMessage:
    handlePayload(p, static_cast<SfmMsgTetra_rx *>(msg.data()));
    break;
  case SfmMessage::MsgId::LicenseState:
  case SfmMessage::MsgId::TacticalState:
  default:
    WARNING_LOG(m_logger) << "RX(WLS): MessageID(" << static_cast<int>(msg->getId()) << ")";
    break;
  }
  return true;
}


void WlsHandler::handlePayload(WlsParticipant *participant, const SfmMsgEquipmentId *sfmTele)
{
  const int equipmentNumber = sfmTele->getEquipmentNumber();
  INFO_LOG(m_logger) << participant->getPlayerIdStr() << ": handlePayload"
                     << " - new equipmentNumber = " << equipmentNumber;
  if (participant->equipmentNumber() == 0 ||
      participant->equipmentNumber() != equipmentNumber)
  {
    m_participantHandler->setEquipmentNumber(participant, equipmentNumber);
  }
}

void WlsHandler::handlePayload(WlsParticipant *participant, const SfmMsgRequestGpsControl_rx *sfmTele)
{
  Q_UNUSED(sfmTele);
  sendGpsMode(participant);
}

void WlsHandler::handlePayload(WlsParticipant *participant, const SfmMsgSfmRestart_rx *sfmTele)
{
  WARNING_LOG(m_logger) << "Restart notification ... " << participant->tagId();
  Q_UNUSED(sfmTele)

  participant->setWlsZone(-1);
  // TODO: noch EXCON informieren?
}

void WlsHandler::handlePayload(WlsParticipant *participant, const Acknowledge_rx *sfmTele)
{
  participant->handleWlsAck(sfmTele);
}

void WlsHandler::handlePayload(WlsParticipant *participant, const SfmMsgTetra_rx *sfmTele)
{
  Acknowledge_tx msg;
  if (!participant->handleSfmRxData(sfmTele))
    msg.setFragmentError(participant->nextSfmRxDataFragmentCounter());
  msg.setRecvCounter(sfmTele->sendCounter());
  sendWlsMessage(participant->tagId(), msg);
}

void WlsHandler::sendGpsMode(WlsParticipant *participant)
{
  if (!participant)
    return;
  SfmMsgGpsControl_tx msg;
  msg.setGpsOn(participant->isGpsMode());
  INFO_LOG(m_logger) << participant->getPlayerIdStr() << ": sendGpsMode"
                     << " GpsMode = "  << participant->isGpsMode();
  sendWlsMessage(participant->tagId(), msg);
}

bool WlsHandler::parseDoubleCm(const QByteArray &data, double &num) const
{
  bool bOk;
  num = data.toDouble(&bOk);
  if (!bOk)
  {
    WARNING_LOG(m_logger) << "Error parsing floating point number '" << data << "'";
    return false;
  }
  num /= 100; // cm -> m
  return true;
}

bool WlsHandler::parseInteger(const QByteArray &data, int &num) const
{
  bool bOk;
  num = data.toInt(&bOk);
  if (!bOk)
  {
    WARNING_LOG(m_logger) << "Error parsing number '" << data << "'";
    return false;
  }
  return true;
}

WlsParticipant *WlsHandler::participantFromTagId(const QByteArray &tagId, bool bAskForEqIdIfNew)
{
  WlsParticipant *p = m_participantHandler->participantByTagId(tagId);
  bool bRequestEquipmentNumber = false;
  const qint64 tNow = QDateTime::currentMSecsSinceEpoch();
  if (!p)
  {
    p = m_participantHandler->createNewParticipant(tagId);
    bRequestEquipmentNumber = bAskForEqIdIfNew;
  }
  else
  {
    // Wenn noch keine EquipmentNumber gekommen ist oder die letzte Anfrage dazu älter als 10 Sekunden ist
    if (p->equipmentNumber() <= 0 && p->lastEquipmentNumberRequest < tNow - 10 * 1000)
      bRequestEquipmentNumber = true;
  }
  if (bRequestEquipmentNumber)
  {
    // TODO: wirklich nur wenn der Teilnehmer noch nicht vorhanden ist?
    // was, wenn wir das Antworttelegramm nicht bekommen? Dann wird aktuell nie eine AusstgNr hinzugefügt
    // -> Manfred Gronak fragen
    SfmMsgRequestEquipmentId_tx getEqId;
    INFO_LOG(m_logger) << "Requesting Equipment number for tag " << tagId;
    sendWlsMessage(tagId, getEqId);
    p->lastEquipmentNumberRequest = tNow;
  }

  return p;
}

std::ostream &operator<<(std::ostream &stream, WlsHandler::WlsTelegramId ti)
{
  switch (ti)
  {
  case WlsHandler::WlsTelegramId::VersionInformation:
    return stream << "WlsTelegramId::VersionInformation";
  case WlsHandler::WlsTelegramId::PlayerPosition:
    return stream << "WlsTelegramId::PlayerPosition";
  case WlsHandler::WlsTelegramId::PlayerState:
    return stream << "WlsTelegramId::PlayerState";
  case WlsHandler::WlsTelegramId::PayloadMobileToHost:
    return stream << "WlsTelegramId::PayloadMobileToHost";
  case WlsHandler::WlsTelegramId::PayloadHostToMobile:
    return stream << "WlsTelegramId::PayloadHostToMobile";
  }
  return stream << "Unknown WlsTelegramId (" << std::to_string(int(ti)) << ")";
}
} // namespace WirelessLocationSystem