/**
  * Canag Handler for de/encoding LtsUtils::SfmMessage
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Philipp Dassow, NLST, 2018-01-29
  */

#include "canag_handler.h"

#include <LtsUtilsCore/dds_vector.h>
#include <LtsUtilsCore/qbytearray_byteaccess.h>

#include "connection_handler.h"
#include "main_handler.h"
#include "settings.h"

namespace TrainingAreaBaseOC
{

TxDdlHandler::TxDdlHandler()
  : m_state(DdlState::NotStarted)
  , m_segmentToggle(0)
  , m_sentCnt(0)
  , m_responseTimer(new QTimer(this))
  , m_logger(getLogger("TxDdlHandler"))
{
  m_responseTimer->setInterval(1000);
  m_responseTimer->setSingleShot(true);
  connect(m_responseTimer, &QTimer::timeout, this, &TxDdlHandler::sendAbortOrRestartTimeout);
}

void TxDdlHandler::addData(const QByteArray &data)
{
  if (data.size() == 0)
    return;

  DdlData ddlData;
  ddlData.data = data;

  m_dataToSend.append(ddlData);

  startNextDomainDownload();
}

void TxDdlHandler::process(const QByteArray &data)
{
  m_responseTimer->stop();

  qint32 scs = ((data[0] & 0xE0) >> 5);
  switch (scs)
  {
  case 1:
  {
    DEBUG_LOG(m_logger) << "Segment download confirmation received";
    // confirmation domain download
    qint32 toggle = ((data[0] & 0x10) >> 4);

    if (m_segmentToggle != toggle)
    {
      ERROR_LOG(m_logger) << "Toggle bit of confirmation not equal to request";
      sendAbort(130);
    }

    (0 == m_segmentToggle) ? m_segmentToggle = 1 : m_segmentToggle = 0;
    domainDownload();
    break;
  }
  case 3:
  {
    DEBUG_LOG(m_logger) << "Initiate download confirmation received";
    // confirmation initate download
    if (m_dataToSend.isEmpty())
      break;

    if (DdlState::Initiated != m_state)
    {
      ERROR_LOG(m_logger) << "Initiate download confirmation received but ddl wasn't startet yet.";
      sendAbort();
      break;
    }
    domainDownload();
    break;
  }
  case 4:
  {
    WARNING_LOG(m_logger) << "Error during domain download";

    m_state = DdlState::NotStarted;
    if (m_dataToSend.isEmpty())
      break;
    // abort download and send again
    m_dataToSend.first().offset = 0;
    startNextDomainDownload();
    break;
  }
  default:
    // error
    ERROR_LOG(m_logger) << "unknown scs value received: " << scs;
    break;
  }
}

void TxDdlHandler::startNextDomainDownload()
{
  if (m_dataToSend.isEmpty() || (DdlState::NotStarted != m_state)) // no DDL Running
    return;

  initiateDomainDownload();
}

void TxDdlHandler::sendAbortOrRestartTimeout()
{
  if (m_state != DdlState::NotStarted)
    sendAbort();
  else
    startNextDomainDownload();
}

void TxDdlHandler::initiateDomainDownload()
{
  if (m_dataToSend.isEmpty())
  {
    ERROR_LOG(m_logger) << "::initiateDomainDownload called, but no data to send available";
    return;
  }

  if (m_sentCnt >= s_maxResendCnt)
  {
    ERROR_LOG(m_logger) << "Maximum trys of resending DDL exceeded. ";
    m_dataToSend.removeFirst();
    m_sentCnt = 0;
    startNextDomainDownload();
    return;
  }

  quint32 dataSize = m_dataToSend.first().data.size();

  QByteArray data(8, '\0');
  data[0] = 0x21;
  data[4] = (dataSize & 0xFF);
  data[5] = ((dataSize >> 8) & 0xFF);
  data[6] = ((dataSize >> 16) & 0xFF);
  data[7] = ((dataSize >> 24) & 0xFF);

  DEBUG_LOG(m_logger) << "Initiate DDL Req";

  m_state = DdlState::Initiated;
  m_sentCnt++;
  emit send(data);
  m_responseTimer->start();
}

void TxDdlHandler::domainDownload()
{
  if (m_dataToSend.isEmpty())
  {
    ERROR_LOG(m_logger) << "::domainDownload called, but no data to send available";
    return;
  }

  // if data alread sent!
  // confirmation received, delete data
  if (m_dataToSend.first().data.size() <= m_dataToSend.first().offset)
  {
    m_dataToSend.removeFirst();
    m_sentCnt = 0;
    m_state = DdlState::NotStarted;
    startNextDomainDownload();
    return;
  }

  QByteArray data(8, '\0');

  qint32 numFreeBytes = 0;
  qint32 endBit = 0;

  qint32 dataSize = m_dataToSend.first().data.size();
  qint32 offset = m_dataToSend.first().offset;
  qint32 newOffset = m_dataToSend.first().offset + 7;

  if (0 == offset)
    m_segmentToggle = 0;

  if (dataSize > newOffset)
  {
    numFreeBytes = 0;
    endBit = 0;
  }
  else if (dataSize == newOffset)
  {
    numFreeBytes = 0;
    endBit = 1;
  }
  else
  {
    numFreeBytes = 7 - (dataSize - offset);
    endBit = 1;
  }

  data[0] = (((m_segmentToggle << 4) & 0x10) | ((numFreeBytes << 1) & 0x0E) | (endBit & 0x01));

  for (qint32 i = 0; i < 7; i++)
  {
    if (m_dataToSend.first().data.size() >= offset + i)
      data[i + 1] = m_dataToSend.first().data[offset + i];
  }

  m_dataToSend.first().offset = newOffset;

  DEBUG_LOG(m_logger) << "Segment DDL Req";
  m_state = DdlState::Downloading;

  emit send(data);
  m_responseTimer->start();
}

void TxDdlHandler::sendAbort(quint8 error)
{
  QByteArray data(8, '\0');
  data[0] = 0x80;
  data[1] = error;
  DEBUG_LOG(m_logger) << "Abort DDL Req";

  m_state = DdlState::NotStarted;
  m_dataToSend.first().offset = 0;
  emit send(data);
  m_responseTimer->start();
}

RxDdlHandler::RxDdlHandler(bool isMultiplexed)
  : m_isMultiplexed(isMultiplexed)
  , m_logger(getLogger("RxDdlHandler"))
{}

void RxDdlHandler::process(const QByteArray &data)
{
  unsigned int ccs = ((data[0] & 0xE0) >> 5);

  switch (ccs)
  {
  //initiate domain download
  case 1:
  {
    DEBUG_LOG(m_logger) << "initiate download received";
    if (m_isMultiplexed)
      m_multiplexer = (((data[1] & 0xFF) << 16) | ((data[2] & 0xFF) << 8) | (data[3] & 0xFF));

    m_messageData.clear();

    // send confirmation
    if (m_state == DdlState::NotStarted)
      sendDPConfirmation(ccs, 0, m_multiplexer);
    else
      sendDPConfirmation(4, 0);
    break;
  }
  //download segment
  case 0:
  {
    DEBUG_LOG(m_logger) << "download segment received";
    if (m_state == DdlState::NotStarted)
    {
      WARNING_LOG(m_logger) << "download segment and data m_state == DdlState::NotStarted, something went wrong!";
      sendDPConfirmation(4, 0);
      return;
    }

    unsigned int toggle = ((data[0] & 0x10) >> 4);
    unsigned int freebytes = ((data[0] & 0x0E) >> 1);

    for (unsigned int i = 1; i < 8 - freebytes; i++)
      m_messageData.append(data.at(i));

    unsigned int dataend = (data[0] & 0x01);

    // send confirmation
    sendDPConfirmation(ccs, toggle);

    // last data package,
    if (1 == dataend)
    {
      if (m_isMultiplexed)
        emit receivedMultiplexed(m_messageData, m_multiplexer);
      else
        emit received(m_messageData);
      m_state = DdlState::NotStarted;
    }
    break;
  }
  //abort download
  case 4:
  {
    DEBUG_LOG(m_logger) << "abort download received";
    m_messageData.clear();
    m_state = DdlState::NotStarted;
    break;
  }
  default:
  {
    WARNING_LOG(m_logger) << "Unknown ccs ID: " << ccs;
    break;
  }
  }
}

void RxDdlHandler::sendDPConfirmation(const quint32 ccs, const quint32 toggle, const quint32 wirkfeldMultiplex)
{
  QByteArray data(8, '\0');

  // initiate download
  if (1 == ccs)
  {
    // scs bit 7-5 = 1
    data[0] = 0x60;
    m_state = DdlState::Initiated;
  } // download segment
  else if (0 == ccs)
  {
    if (0 == toggle)
      data[0] = 0x20;
    else if (1 == toggle)
      data[0] = 0x30;
    else
      WARNING_LOG(m_logger) << "Unknown toggle state: " << toggle;
    m_state = DdlState::Downloading;
  }
  else if (4 == ccs)
  {
    data[0] = 0x80;
    m_state = DdlState::NotStarted;
  }

  if (0 != wirkfeldMultiplex)
  {
    data[1] = ((wirkfeldMultiplex >> 16) & 0xFF);
    data[2] = ((wirkfeldMultiplex >> 8) & 0xFF);
    data[3] = (wirkfeldMultiplex & 0xFF);
  }
  emit send(data);
}

CanagHandler::CanagHandler(MainHandler *mainHandler)
  : m_mainHandler(mainHandler)
  , m_connectionHandler(mainHandler->connectionHandler())
  , m_rxHandler(mainHandler->rxHandler())
  , m_freeText(QByteArray())
  , m_deviceMessageTimer(new QTimer(this))
  , m_freeTextCollectionTimer(new QTimer(this))
  , m_sfmAliveTimer(new QTimer(this))
  , m_sfmConnectionCheckTimer(new QTimer(this))
  , m_syncSocket(new QTcpSocket(this))
  , m_technicalStateSfm(OcTelegrams::TechnicalStateSfm())
  , m_sfmConnectionEstablished(false)
  , m_logger(getLogger("CanagHandler"))
{}

CanagHandler::~CanagHandler()
{
  delete m_deviceMessageTimer;
  delete m_freeTextCollectionTimer;
  delete m_sfmAliveTimer;
  delete m_sfmConnectionCheckTimer;
  qDeleteAll(m_txDdlHandler);
  qDeleteAll(m_rxDdlHandler);
  qDeleteAll(m_messages);
}

bool CanagHandler::init()
{
  m_handlerAesData = std::make_shared<HandlerAesData>(m_mainHandler->dds(), m_mainHandler->aesDtc(), m_mainHandler->aesFtc(), m_mainHandler->pdmDtc());
  m_handlerAesData->setUse2BytesForSingleFireDiameter(Settings::use2BytesForSingleFireDiameter());

  if (!m_handlerAesData->init())
  {
    ERROR_LOG(m_logger) << "Could not initialise HandlerAesData.";
    return false;
  }

  m_deviceMessageTimer->setSingleShot(true);
  connect(m_deviceMessageTimer, &QTimer::timeout, this, &CanagHandler::sendDeviceMessage);
  m_freeTextCollectionTimer->setInterval(1000);
  connect(m_freeTextCollectionTimer, &QTimer::timeout, this, &CanagHandler::sendFreeText);
  m_sfmAliveTimer->setSingleShot(true);
  m_sfmAliveTimer->setInterval(6000);
  connect(m_connectionHandler.get(), &ConnectionHandler::disconnectedFromEq, this, &CanagHandler::sfmAliveTimeout);
  connect(m_sfmAliveTimer, &QTimer::timeout, this, &CanagHandler::sfmAliveTimeout);
  m_sfmConnectionCheckTimer->setInterval(5000);
  connect(m_sfmConnectionCheckTimer, &QTimer::timeout, this, &CanagHandler::sendConnectionTestSfm);
  connect(m_connectionHandler.get(), &ConnectionHandler::received, this, &CanagHandler::handleSfmTelegram);

  // TX DDL-Handler
  // OC->EXCON
  TxDdlHandler *txDdlHandler = new TxDdlHandler;
  connect(txDdlHandler, &TxDdlHandler::send, this, &CanagHandler::sendOcExcon);
  m_txDdlHandler.insert(CanId::OcExconDataTx, txDdlHandler);
  // FireSmokeMarker->EXCON
  txDdlHandler = new TxDdlHandler;
  connect(txDdlHandler, &TxDdlHandler::send, this, &CanagHandler::sendFireSmokeMarkerExcon);
  m_txDdlHandler.insert(CanId::FireSmokeMarkerExconDataTx, txDdlHandler);

  // RX DDL-Handler
  // EXCON->OC
  RxDdlHandler *rxDdlHandler = new RxDdlHandler;
  connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendExconOc);
  connect(rxDdlHandler, &RxDdlHandler::received, this, &CanagHandler::receivedExconOc);
  m_rxDdlHandler.insert(CanId::ExconOcDataTx, rxDdlHandler);
  // EXCON->FireSmokeMarker
  rxDdlHandler = new RxDdlHandler;
  connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendExconFireSmokeMarker);
  connect(rxDdlHandler, &RxDdlHandler::received, this, &CanagHandler::receivedExconFireSmokeMarker);
  m_rxDdlHandler.insert(CanId::ExconFireSmokeMarkerDataTx, rxDdlHandler);
  // Excon->FireSmokeMarker
  rxDdlHandler = new RxDdlHandler(true);
  connect(rxDdlHandler, &RxDdlHandler::send, this, &CanagHandler::sendSfmAwes);
  connect(rxDdlHandler, &RxDdlHandler::receivedMultiplexed, this, &CanagHandler::receivedSfmAwes);
  m_rxDdlHandler.insert(CanId::AwesDataTx, rxDdlHandler);

  fillMessageMap();

  m_sfmConnectionCheckTimer->start();
  openSocket();
  return true;
}

bool CanagHandler::txData(const OcTelegrams::DemandCyclicPositions &data)
{
  LtsUtils::SfmMessage msg;
  msg.setId(CanId::DemandCyclicPosition);

  QByteArray outData(1, '\0');
  outData[0] = data.cycleTime;
  msg.setPayload(outData);

  send(msg);
  return true;
}

bool CanagHandler::txData(const OcTelegrams::TechnicalStateOc &data)
{
  LtsUtils::SfmMessage *msg = m_messages[CanId::TechnicalStateOc];

  QByteArray outData(8, '\0');
  unsigned int unixTime = m_mainHandler->dds()->currentPartitionTime() / 1000;
  outData[0] = (unixTime >> 24  & 0xFF);
  outData[1] = (unixTime >> 16  & 0xFF);
  outData[2] = (unixTime >> 8 & 0xFF);
  outData[3] = (unixTime >> 0 & 0xFF);
  outData[7] = data.errorCode;
  msg->setPayload(outData);

  send(*msg);
  return true;
}

bool CanagHandler::txData(const OcTelegrams::DemandSelftestSfm &data)
{
  Q_UNUSED(data);
  send(*m_messages[CanId::ParameterSfmTx]);

  return true;
}

bool CanagHandler::txData(const TrainingAreaBase::TetraIcd::OpaquePayloadDataBase &data)
{
  switch (data.payloadType)
  {
  case TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::OC:
  {
    INFO_LOG(m_logger) << "adding DDL CanId::CAN_ID_DATENVERKEHR_SRT_KOPPELRECHNER_TX";
    m_txDdlHandler[CanId::OcExconDataTx]->addData(LtsUtils::toQByteArray(data.payload));
    break;
  }
  case TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::FireSmokeMarker:
  {
    INFO_LOG(m_logger) << "adding DDL CanId::CAN_ID_DATENVERKEHR_DMF_KOPPELRECHNER_TX";
    m_txDdlHandler[CanId::FireSmokeMarkerExconDataTx]->addData(LtsUtils::toQByteArray(data.payload));
    break;
  }
  default:
  {
    ERROR_LOG(m_logger) << "Tryed to send unhandled TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type " << static_cast<int>(data.payloadType);
    return false;
  }
  }

  return true;
}

void CanagHandler::handleSfmTelegram(const LtsUtils::SfmMessage &message)
{
  if (message.id() == 0)
  {
    ERROR_LOG(m_logger) << "Error while receiving data from sfm.";
    return;
  }

  DEBUG_LOG(m_logger) << "Received new CAN-Message with ID: " << message.id() << " RTR: " << message.rtr() << " data: " << message.payload().toHex(' ');
  switch (message.id())
  {
  case CanId::DemandDeviceId:
  {
    INFO_LOG(m_logger) << "Received CanId::DemandDeviceId";
    m_deviceMessageTimer->start();
    break;
  }
  case CanId::Time:
  {
    DEBUG_LOG(m_logger) << "Received CanId::Time";
    QByteArray data = message.payload();
    OcTelegrams::TimeSync outData;

    uint32_t gpsTime;
    LtsUtils::get4Byte(data, gpsTime);
    outData.gpsTimeMsec = gpsTime * 1000ULL;
    outData.timezone = data[4];
    DEBUG_LOG(m_logger) << "Received " << gpsTime;
    sendSyncTime(gpsTime);

    m_rxHandler->rxData(outData);
    break;
  }
  case CanId::EnterLeaveAes:
  {
    INFO_LOG(m_logger) << "Received CanId::EnterLeaveAes";
    QByteArray data = message.payload();
    OcTelegrams::EnterLeaveAes outData;

    // If "Operational Area", return false, since Entering/Exiting OAs is not handled
    // on O/C Laptop
    bool is_NOT_OperationalArea;
    // ICD CANbus-Telegramme Page 85
    outData.aesId = data.mid(0, 2).toUInt();
    outData.aesEntered = (data[2] & 0x80) >> 7;
    outData.nbcArea = (data[2] & 0x40) >> 6;
    is_NOT_OperationalArea = (data[2] & 0x38) == 0;
    outData.chargeTasCode = data[3];
    outData.cAreaType = data[4];

    if (is_NOT_OperationalArea)
      m_rxHandler->rxData(outData);
    break;
  }
  case CanId::ParameterOcRx:
  {
    INFO_LOG(m_logger) << "Received CanId::ParameterOcRx";
    send(*m_messages[CanId::ParameterOcTx]);
    break;
  }
  case CanId::TechnicalStateSfm:
  {
    INFO_LOG(m_logger) << "Received CanId::TechnicalStateSfm";
    QByteArray data = message.payload();
    OcTelegrams::TechnicalStateSfm outData;

    outData.sfmConnected = true;

    // ICD CANbus-Telegramme Page 78

    uint32_t gpsTime;
    LtsUtils::get4Byte(data, gpsTime);
    outData.gpsTimeMsec = gpsTime * 10000ULL;
    outData.batteryTest = (data[4] & 0x80) >> 7;
    outData.radioAvailable = !((data[4] & 0x40) >> 6);
    outData.manipulationEquipment = (data[4] & 0x20) >> 5;
    outData.licensed = !((data[4] & 0x10) >> 4);
    outData.manipulationSfm = (data[4] & 0x01);
    outData.errorCode = data[5];

    m_technicalStateSfm = outData;

    m_rxHandler->rxData(outData);
    break;
  }
  case CanId::TechnicalStateOc:
  {
    INFO_LOG(m_logger) << "Received CanId::TechnicalStateOc";
    // reqest TECHNISCHER_STATUS_SRT
    if (message.rtr())
      send(*m_messages[CanId::TechnicalStateOc]);
    break;
  }
  case CanId::ExconOcDataRx:
  {
    INFO_LOG(m_logger) << "Received CanId::ExconOcDataRx";
    QByteArray data = message.payload();
    m_rxDdlHandler[CanId::ExconOcDataTx]->process(data);
    break;
  }
  case CanId::OcExconDataRx:
  {
    INFO_LOG(m_logger) << "Received CanId::OcExconDataRx";
    QByteArray data = message.payload();
    m_txDdlHandler[CanId::OcExconDataTx]->process(data);
    break;
  }
  case CanId::ExconFireSmokeMarkerDataRx:
  {
    INFO_LOG(m_logger) << "Received CanId::ExconFireSmokeMarkerDataRx";
    QByteArray data = message.payload();
    m_rxDdlHandler[CanId::ExconFireSmokeMarkerDataTx]->process(data);
    break;
  }
  case CanId::FireSmokeMarkerExconDataRx:
  {
    INFO_LOG(m_logger) << "Received CanId::FireSmokeMarkerExconDataRx";
    QByteArray data = message.payload();
    m_txDdlHandler[CanId::FireSmokeMarkerExconDataTx]->process(data);
    break;
  }
  case CanId::AwesDataRx:
  {
    INFO_LOG(m_logger) << "Received CanId::AwesDataRx";
    QByteArray data = message.payload();
    m_rxDdlHandler[CanId::AwesDataTx]->process(data);
    break;
  }
  case CanId::Position:
  {
    INFO_LOG(m_logger) << "Received CanId::Position";
    QByteArray data = message.payload();
    OcTelegrams::OwnPosition outData;

    // ICD CANbus-Telegramme Page 86
    outData.valid = ((data[0] & 0x80) == 0);
    size_t pos = 2;
    LtsUtils::get3Byte(data, pos, outData.northing);
    LtsUtils::get3Byte(data, pos, outData.easting);
     if (!outData.valid)
       break;
    outData.gpsTimeMsec = m_mainHandler->dds()->currentPartitionTime();
    m_rxHandler->rxData(outData);
    break;
  }
  case CanId::DisplayMessage:
  {
    INFO_LOG(m_logger) << "Received CanId::DisplayMessage";

    handleDisplayMessage(message);

    break;
  }
  case CanId::SpecificsExercise:
  {
    INFO_LOG(m_logger) << "Received CanId::SpecificsExercise";
    QByteArray data = message.payload();
    OcTelegrams::SpecificsExercise outData;
    // ICD CANbus-Telegramme Page 80
    size_t pos = 0;
    LtsUtils::get2Byte(data, pos, outData.exerciseId);
    outData.zone = data[2];
    outData.band = data[3];
    outData.square = data.mid(4, 2);
    pos = 6;
    LtsUtils::get2Byte(data, pos, outData.easting);
    outData.easting *= 1000;
    LtsUtils::get2Byte(data, pos, outData.northing);
    outData.northing *= 1000;

    Settings::baseCoord(QString::number(outData.zone) +
                        QLatin1Char(outData.band) +
                        QString::fromLatin1(outData.square) +
                        QString::number(outData.northing).rightJustified(7, QLatin1Char('0')) +
                        QString::number(outData.easting).rightJustified(7, QLatin1Char('0')));

    m_handlerAesData->specificsExercise(outData);

    m_rxHandler->rxData(outData);
    break;
  }
  case CanId::SpecificsOc:
  {
    INFO_LOG(m_logger) << "Received CanId::SpecificsOc";
    // request Kenndaten SRT?
    if (message.rtr())
      send(*m_messages[CanId::SpecificsOc]);
    break;
  }
  case CanId::SpecificsEquipment:
  {
    INFO_LOG(m_logger) << "Received CanId::SpecificsEquipment";
    QByteArray data = message.payload();
    OcTelegrams::SpecificsEquipment outData;

    // ICD CANbus-Telegramme Page 81
    LtsUtils::get2Byte(data, outData.equipmentNumber);
    outData.playerType = data[2];
    outData.language = static_cast<TrainingAreaBase::TetraIcd::Language>(data.at(3));
    outData.pyrotechnics = (data[4] & 0x80) >> 7;
    outData.voiceOutput = (data[4] & 0x40) >> 6;
    outData.sfm = (data[4] & 0x30) >> 4;
    outData.panCoordinator = (data[4] & 0x0C) >> 2;
    outData.viss = (data[4] & 0x03);
    outData.geopairing = (data[5] & 0xC0) >> 6;
    outData.ecu = (data[5] & 0x30) >> 4;
    outData.hsu = (data[5] & 0x0C) >> 2;
    outData.halt = (data[5] & 0x03);

    outData.exerciseBreak = (data[5] & 0x02);

    m_rxHandler->rxData(outData);
    break;
  }
  case CanId::ParameterSfmRx:
  {
    INFO_LOG(m_logger) << "Received CanId::ParameterSfmRx";
    // Do nothing for CanId::ParameterSfmRx as it is just a reply
    break;
  }
  case CanId::ConnectionTestSfmReply:
  {
    INFO_LOG(m_logger) << "Received CanId::TestConnectionSfmReply";
    if (!m_sfmConnectionEstablished)
      sendDemandDeviceId();
    else
      m_sfmAliveTimer->start();

    break;
  }
  default:
    DEBUG_LOG(m_logger) << "message id " << message.id() << " is not handled!";
    break;
  }
}

void CanagHandler::sendDemandDeviceId()
{
  INFO_LOG(m_logger) << "Requesting Device ID.";
  send(*m_messages[CanId::DemandDeviceId]);
  m_deviceMessageTimer->start();
}

void CanagHandler::sendDeviceMessage()
{
  INFO_LOG(m_logger) << "Sending Device ID.";
  send(*m_messages[CanId::DeviceId]);
  if (!m_technicalStateSfm.sfmConnected)
    demandInitialSfmData();
}

void CanagHandler::sendOcExcon(const QByteArray &data)
{
  send(CanId::OcExconDataTx, data);
}

void CanagHandler::sendFireSmokeMarkerExcon(const QByteArray &data)
{
  send(CanId::FireSmokeMarkerExconDataTx, data);
}

void CanagHandler::sendExconOc(const QByteArray &data)
{
  send(CanId::ExconOcDataTx, data);
}

void CanagHandler::sendExconFireSmokeMarker(const QByteArray &data)
{
  send(CanId::ExconFireSmokeMarkerDataTx, data);
}

void CanagHandler::sendSfmAwes(const QByteArray &data)
{
  send(CanId::AwesDataTx, data);
}

void CanagHandler::sendConnectionTestSfm()
{
  if (m_connectionHandler->isOpen())
  {
    m_sfmAliveTimer->start();
    send(*m_messages[CanId::ConnectionTestSfm]);
  }
    
}

void CanagHandler::receivedExconOc(const QByteArray &data)
{
  TrainingAreaBase::TetraIcd::OpaquePayloadDataOut outData;
  outData.payloadType = TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::OC;
  outData.payload = LtsUtils::convert<std::vector<unsigned char>>(data);

  m_rxHandler->rxData(outData);
}

void CanagHandler::receivedExconFireSmokeMarker(const QByteArray &data)
{
  TrainingAreaBase::TetraIcd::OpaquePayloadDataOut outData;
  outData.payloadType = TrainingAreaBase::TetraIcd::OpaquePayloadDataBase::Type::FireSmokeMarker;
  outData.payload = LtsUtils::convert<std::vector<unsigned char>>(data);

  m_rxHandler->rxData(outData);
}

void CanagHandler::receivedSfmAwes(const QByteArray &data, quint32 multiplexer)
{
  m_handlerAesData->process(data, multiplexer);
}

void CanagHandler::sfmAliveTimeout()
{
  m_technicalStateSfm.sfmConnected = false;
  m_technicalStateSfm.radioAvailable = false;
  m_technicalStateSfm.licensed = false;
  m_sfmConnectionEstablished = false;

  // not really received but we have to update the connection state
  m_rxHandler->rxData(m_technicalStateSfm);
}

void CanagHandler::sendFreeText()
{
  m_freeTextCollectionTimer->stop();

  OcTelegrams::Display outData;
  int length = m_freeText.length() - 1; // -1 as always cut of the first byte
  if (*(m_freeText.constEnd()) == 0x03) // normal end
    length -= 1; // -1 -> remove ETX
  outData.multiplexer = 1;
  outData.text = m_freeText.mid(1, length);
  m_freeText.clear();

  m_rxHandler->rxData(outData);
}

void CanagHandler::sendSyncTime(uint32_t syncTimeSec)
{
  INFO_LOG(m_logger) << "sendSyncTime " << syncTimeSec;

  if (syncTimeSec > QDateTime::currentSecsSinceEpoch() - 3 &&
      syncTimeSec < QDateTime::currentSecsSinceEpoch() + 3)
  {
    DEBUG_LOG(m_logger) << "PartitionTime is Still valid";
    return;
  }
  if (!m_syncSocket->isOpen())
    openSocket();

  if (m_syncSocket->isOpen())
  {
    m_syncSocket->write(QByteArray::number(syncTimeSec));
    m_syncSocket->flush();
    DEBUG_LOG(m_logger) << "Send: " << syncTimeSec << " " << QByteArray::number(syncTimeSec);
  }
}

void CanagHandler::send(const LtsUtils::SfmMessage &message)
{
  // No WakeUps, as they are not needed and only cause problems

  DEBUG_LOG(m_logger) << "send message - " << message.id() << " RTR: " << message.rtr();
  if (!message.payload().isEmpty())
    DEBUG_LOG(m_logger) << " size: " << message.payload().size() << " data: " <<  message.payload().toHex(' ');


  if (!m_connectionHandler->send(message))
    ERROR_LOG(m_logger) << "Could not send message!";
  else
    emit sendMessage(message);
}

void CanagHandler::send(quint32 canId, const QByteArray &data)
{
  LtsUtils::SfmMessage msg;
  msg.setId(canId);
  msg.setPayload(data);
  send(msg);
}

void CanagHandler::openSocket()
{
  if (m_syncSocket->isOpen())
    return;

  m_syncSocket->connectToHost(Settings::hostName(), 13777);
  if (!m_syncSocket->waitForConnected())
    WARNING_LOG(m_logger) << "Could not (re)open tcp destination " << m_syncSocket->peerName() << ":" << m_syncSocket->peerPort() << ", error: " << m_syncSocket->errorString();
  else
  {
    connect(m_syncSocket, &QTcpSocket::disconnected, this, [this]
    {
      DEBUG_LOG(m_logger) << "Disconnected from Host";
      m_syncSocket->close();
    });
    DEBUG_LOG(m_logger) << "(re)open tcp destination " << m_syncSocket->peerName() << ":" << m_syncSocket->peerPort();
  }
}

void CanagHandler::fillMessageMap()
{
  m_messages.clear();

  // DemandDeviceId
  LtsUtils::SfmMessage *message = new LtsUtils::SfmMessage();
  message->setId(CanId::DemandDeviceId);
  m_messages.insert(CanId::DemandDeviceId, message);

  // DeviceId
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::DeviceId);
  QByteArray dataDeviceId = QByteArray(4, '\0');
  dataDeviceId[0] = Settings::manufacturerId();
  dataDeviceId[1] = Settings::deviceId();
  dataDeviceId[2] = (Settings::serialNumber() << 8) & 0xFF;
  dataDeviceId[3] = Settings::manufacturerId() & 0xFF;
  message->setPayload(dataDeviceId);
  m_messages.insert(CanId::DeviceId, message);

  // TechnicalStateOc
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::TechnicalStateOc);
  QByteArray dataTechSrt(8, '\0');
  unsigned int unixTime = m_mainHandler->dds()->currentPartitionTime() / 1000;
  dataTechSrt[0] = (unixTime >> 24  & 0xFF);
  dataTechSrt[1] = (unixTime >> 16  & 0xFF);
  dataTechSrt[2] = (unixTime >> 8 & 0xFF);
  dataTechSrt[3] = (unixTime >> 0 & 0xFF);
  //data[7] = 0;   // default no error
  message->setPayload(dataTechSrt);
  m_messages.insert(CanId::TechnicalStateOc, message);

  // SpecificsOc
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::SpecificsOc);
  QByteArray dataSpecificsOc = QByteArray(5, '\0');
  dataSpecificsOc[0] = (((Settings::verX() & 0x3F) << 2) | ((Settings::verY() & 0x18) >> 3));
  dataSpecificsOc[1] = (((Settings::verY() & 0x07) << 5) | (Settings::verZ() & 0x1F));
  dataSpecificsOc[2] = (Settings::biteTimeOut() >> 8) & 0xFF;
  dataSpecificsOc[3] = Settings::biteTimeOut() & 0xFF;
  dataSpecificsOc[4] = 1; // TODO: get language somewhere
  message->setPayload(dataSpecificsOc);
  m_messages.insert(CanId::SpecificsOc, message);

  // TestConnectionSfm
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::ConnectionTestSfm);
  m_messages.insert(CanId::ConnectionTestSfm, message);

  // ParameterSfmTx
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::ParameterSfmTx);
  QByteArray dataParaTne(3, '\0');
  dataParaTne[0] = 0;
  dataParaTne[1] = 1;
  dataParaTne[2] = 0;
  message->setPayload(dataParaTne);
  m_messages.insert(CanId::ParameterSfmTx, message);

  // ParameterOcTx
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::ParameterOcTx);
  QByteArray dataParaSrt(2, '\0');
  dataParaSrt[0] = 0;
  dataParaSrt[1] = 0;
  message->setPayload(dataParaSrt);
  m_messages.insert(CanId::ParameterOcTx, message);

  // TechnicalStateSfm RTR
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::TechnicalStateSfm);
  message->setRtr(true);
  m_messages.insert(CanId::TechnicalStateSfm, message);

  // TechnicalStateSfm RTR
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::SpecificsEquipment);
  message->setRtr(true);
  m_messages.insert(CanId::SpecificsEquipment, message);

  // TechnicalStateSfm RTR
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::SpecificsExercise);
  message->setRtr(true);
  m_messages.insert(CanId::SpecificsExercise, message);

  // WakeUp
  message = new LtsUtils::SfmMessage();
  message->setId(CanId::WakeUp);
  m_messages.insert(CanId::WakeUp, message);

}

void CanagHandler::demandInitialSfmData()
{
  sendDemandSpecificsEquipment();
  sendDemandSpecificsExercise();
  sendDemandTechnicalStateSfm();
  OcTelegrams::DemandCyclicPositions data = {};
  data.cycleTime = 1;
  txData(data);
  m_sfmAliveTimer->start();
  m_sfmConnectionCheckTimer->start();
  m_sfmConnectionEstablished = true;
}

void CanagHandler::sendDemandTechnicalStateSfm()
{
  send(*m_messages[CanId::TechnicalStateSfm]);
}

void CanagHandler::sendDemandSpecificsEquipment()
{
  send(*m_messages[CanId::SpecificsEquipment]);
}

void CanagHandler::sendDemandSpecificsExercise()
{
  send(*m_messages[CanId::SpecificsExercise]);
}

void CanagHandler::handleDisplayMessage(const LtsUtils::SfmMessage &message)
{
  QByteArray data = message.payload();
  OcTelegrams::Display outData;

  // ICD CANbus-Telegramme Page 95
  outData.multiplexer = (data[0] & 0x7F);
  if (0 == outData.multiplexer)
  {
    outData.textType = static_cast<OcTelegrams::TextType>(data.at(1));
    size_t pos = 2;
    LtsUtils::get2Byte(data, pos, outData.indexVbg);
    LtsUtils::get2Byte(data, pos, outData.indexVba);

    if (outData.indexVbg != 0 || outData.indexVba != 0) // if indexVbg and indexVba are 0 do not send (was used in old AGDUS displays before free text)
      m_rxHandler->rxData(outData);
  }
  else if (1 == outData.multiplexer)
  {
    if (data.at(1) == 0x02) // STX -> new free text
    {
       if (!m_freeText.isEmpty()) // send pending free text if applicable
        sendFreeText();
    }

    // buffer new text
    m_freeText.append(data.mid(1, data.length() - 1));

    if (data.at(data.length() - 1) == 0x03)
      sendFreeText();
    else
      m_freeTextCollectionTimer->start();
  }
}

} // namespace TrainingAreaBaseOC