/**
  * Handler for all oc situation data at the client
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Philipp Dassow, NLST, 01-03-2018
  */

#include "handler_oc_situation_data.h"

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

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

namespace OcSituationClient
{

HandlerOcSituationData::HandlerOcSituationData(const std::shared_ptr<DdsHandler> &dds,
                                               const std::shared_ptr<ParticipantManagement::Interface::Dtc> &ppmDtc,
                                               const std::shared_ptr<ParticipantManagement::Interface::Ftc> &ppmFtc,
                                               const std::shared_ptr<TrainingAreaBase::Interface::Dts> &tabDts,
                                               const std::shared_ptr<TrainingAreaBaseOC::Interface::Dtc> &tabocDtc,
                                               const std::shared_ptr<TrainingAreaBaseOC::Interface::Dts> &tabocDts,
                                               QObject *parent)
  : QObject(parent)
  , m_dds(dds)
  , m_ppmDtc(ppmDtc)
  , m_ppmFtc(ppmFtc)
  , m_tabDts(tabDts)
  , m_tabocDtc(tabocDtc)
  , m_tabocDts(tabocDts)
  , m_logger(getLogger("HandlerOcSituationData"))
{}

HandlerOcSituationData::~HandlerOcSituationData()
{}

bool HandlerOcSituationData::init()
{
  connect(m_ppmDtc.get(), &ParticipantManagement::Interface::Dtc::partyChanged,
          this, &HandlerOcSituationData::onPartyChanged);
  connect(m_ppmDtc.get(), &ParticipantManagement::Interface::Dtc::participantChanged,
          this, &HandlerOcSituationData::onParticipantChanged);

  // PPM DTC enable
  if (!m_ppmDtc->enableParticipant() ||
      !m_ppmDtc->enableParty() ||
      !m_ppmDtc->enableType())
  {
    ERROR_LOG(m_logger) << "Enabling PPM Data-Topics failed.";
    return false;
  }

  // PPM FTC enable
  if (!m_ppmFtc->enableModifyParticipantData())
  {
    ERROR_LOG(m_logger) << "Enabling PPM Data-Topics failed.";
    return false;
  }

  // TAB DTS enable
  if (!m_tabDts->enableEquipmentPosition() ||
      !m_tabDts->enableEquipmentState() ||
      !m_tabDts->enableEquipmentOperationalState() ||
      !m_tabDts->enableParticipantSanitation() ||
      !m_tabDts->enableSpecialSensorData())
  {
    ERROR_LOG(m_logger) << "Enabling TAB DTS Data-Topics failed.";
    return false;
  }

  // TAOBOC DTC enable
  if (!m_tabocDtc->enableSpecificsEquipment())
  {
    ERROR_LOG(m_logger) << "Enabling TABOC Data-Topics failed.";
    return false;
  }

  if (!m_tabocDts->enableParticipantsOverload())
  {
    ERROR_LOG(m_logger) << "Enabling TABOC fts-Topics failed.";
    return false;
  }


  DEBUG_LOG(m_logger) << "Initialisation HandlerOcSituationData succesful.";
  return true;
}

void HandlerOcSituationData::process(const QByteArray &data, bool bIsInitial)
{
  if (bIsInitial)
  {
    DEBUG_LOG(m_logger) << "Processing initial situation data.";

    int ownEquipmentNumber = m_tabocDtc->specificsEquipment(1).equipmentNumber();
    int ownParticipantId = -1;
    for (const int &participantId : qAsConst(m_participantId2EqId))
    {
      if (m_participantId2EqId.value(participantId) == ownEquipmentNumber)
      {
        ownParticipantId = participantId;
        break;
      } // if
    } // for

    removeAllPositions();
    m_participantData.clear();
    m_equipmentPositions.clear();
    m_equipmentStates.clear();
    m_equipmentOperationalStates.clear();
    m_specialSensorData.clear();
    m_participantId2EqId.clear();

    if (ownParticipantId == -1)
      ERROR_LOG(m_logger) << "Mapping of own participant id and equipment number is unknown.";
    else
      m_participantId2EqId.insert(ownParticipantId, ownEquipmentNumber);
  } // if


  if (!decodeSituationData(data))
    return;

  if (!m_participantData.isEmpty())
    sendParticipantData(++m_requestId);

  sendTrainingAreaBaseData();
}

void HandlerOcSituationData::removeAllPositions()
{
  TrainingAreaBase::EquipmentPosition invalidPos;
  invalidPos.position().position().lon(nan(""));
  invalidPos.position().position().lat(nan(""));
  invalidPos.position().reportingSource(Core::PositionReportingSource::SetByOperator);  // SetByOperator -> nan is allowed
  invalidPos.sampleTime(m_dds->currentPartitionTime());

  for (const auto eqNumber : qAsConst(m_participantId2EqId))
  {
    invalidPos.equipmentNumber(eqNumber);
    m_tabDts->publish(invalidPos);
  }
}

void HandlerOcSituationData::setInvalidPositionForParticipant(int participantId)
{
  TrainingAreaBase::EquipmentPosition invalidPos;
  invalidPos.position().position().lon(nan(""));
  invalidPos.position().position().lat(nan(""));
  invalidPos.position().reportingSource(Core::PositionReportingSource::SetByOperator);  // SetByOperator -> nan is allowed
  invalidPos.sampleTime(m_dds->currentPartitionTime());
  invalidPos.equipmentNumber(-participantId); // negative -> treat equipNo as participantId
  m_tabDts->publish(invalidPos);
}

void HandlerOcSituationData::onModifyParticipantDataReply(const Core::FunctionReplyObject &reply)
{
  if (!m_modifiyRequests.contains(reply.id()))
  {
    ERROR_LOG(m_logger) << "Got a ModifyParticipantDataReply without matching ID!";
    return;
  }

  if (reply.error())
    ERROR_LOG(m_logger) << "ModifyParticipantDataRequest " << reply.id() << " could not be handled: " << reply.str();
  else
  {
    DEBUG_LOG(m_logger) << "ModifyParticipantDataRequest " << reply.id() << " handled successfully by PPM!";
    delete (m_modifiyRequests.value(reply.id()));
    m_modifiyRequests.remove(reply.id());
    sendTrainingAreaBaseData(); // send TAB data for new participants
  }
}

bool HandlerOcSituationData::decodeSituationData(const QByteArray &data)
{
  if (data.isEmpty())
  {
    ERROR_LOG(m_logger) << "Got an empty QByteArray to decode.";
    return false;
  }

  bool participantOverloaded = false;
  LtsUtils::get1Byte(data, participantOverloaded);
  //send overload topic
  TrainingAreaBaseOC::ParticipantsOverload participantsOverload;
  participantsOverload.id(1);
  participantsOverload.hasOverload(participantOverloaded);
  m_tabocDts->publish(participantsOverload);
  DEBUG_LOG(m_logger) << "publish overload: " << participantOverloaded;

  QByteArray data2 = data;
  data2.remove(0, 1);

  // shouldn't be needed in 99,9% of all cases but just to be sure
  m_ppmDtc->waitForHistoricalDataParticipant(5000);
  m_ppmDtc->waitForHistoricalDataParty(5000);
  m_ppmDtc->waitForHistoricalDataType(5000);

  QByteArray uncompressed = MainHandler::tryUncompress(data2, m_bWithCompression);
  if (uncompressed.isEmpty())
  {
    INFO_LOG(m_logger) << "Got an empty QByteArray after uncompression.";
    return false;
  }

  TRACE_LOG(m_logger) << "Uncompressed Data: " << uncompressed.toHex(' ');

  DEBUG_LOG(m_logger) << " m_equipmentPositions size before decode: " << m_equipmentPositions.size()
                      << " m_equipmentStates size before decode: " << m_equipmentStates.size()
                      << " m_equipmentOperationalStates size before decode: " << m_equipmentOperationalStates.size()
                      << " m_specialSensorData size before decode: " << m_specialSensorData.size();

  size_t pos = 0;
  int participantId = 0;
  OcSituation::ModificationBits modifiedFlag;
  int modId = 1000000;
  while (getParticipantId(uncompressed, pos, participantId, modifiedFlag) && participantId != 0)
  {
    INFO_LOG(m_logger) << "Participant: " << participantId << " is modified: " << modifiedFlag;

    // outdated handling
    if (modifiedFlag == OcSituation::ModificationFlag::eNone)
    {
      // directly remove and go on with next participant
      setInvalidPositionForParticipant(participantId);
      continue;
    }

    // position
    bool bRet = false;
    TrainingAreaBase::EquipmentPosition position;
    position.position().reportingSource(Core::PositionReportingSource::Gps);  // we don't know better
    bRet |= getPosition2D(uncompressed, pos, position, (modifiedFlag & OcSituation::ModificationFlag::ePosition2D) == OcSituation::ModificationFlag::ePosition2D);
    bRet |= getPosition3D(uncompressed, pos, position, (modifiedFlag & OcSituation::ModificationFlag::ePosition3D) == OcSituation::ModificationFlag::ePosition3D);
    if (bRet)
      m_equipmentPositions.insert(participantId, position);

    // state
    TrainingAreaBase::EquipmentState state;
    TrainingAreaBase::ParticipantSanitation sanitaion;
    bRet = getVBG(uncompressed, pos, state, sanitaion, (modifiedFlag & OcSituation::ModificationFlag::eVBG) == OcSituation::ModificationFlag::eVBG);
    if (bRet)
    {
      DEBUG_LOG(m_logger) << "Adding EquipmentState to internal map: " << state;
      m_equipmentStates.insert(participantId, state);

      DEBUG_LOG(m_logger) << "Adding ParticipantSanitation to internal map: " << sanitaion;
      m_participantSanitations.insert(participantId, sanitaion);

    }

    // sensorData and operationalState
    TrainingAreaBase::SpecialSensorData sensorData;
    TrainingAreaBase::EquipmentOperationalState opState;
    bRet = getState(uncompressed, pos, sensorData, opState, (modifiedFlag & OcSituation::ModificationFlag::eState) == OcSituation::ModificationFlag::eState);
    if (bRet)
    {
      DEBUG_LOG(m_logger) << "Adding SpecialSensorData to internal map: " << sensorData;
      DEBUG_LOG(m_logger) << "Adding EquipmentOperationalState to internal map: " << opState;
      m_specialSensorData.insert(participantId, sensorData);
      m_equipmentOperationalStates.insert(participantId, opState);
    }

    // participantData
    bRet = false;
    ParticipantManagement::ParticipantData participantData = m_ppmDtc->convertParticipantToParticipantData(m_participants.value(participantId));

    bRet |= getStructure(uncompressed, pos, participantData, (modifiedFlag & OcSituation::ModificationFlag::eStructure) == OcSituation::ModificationFlag::eStructure);
    bRet |= getDesignator(uncompressed, pos, participantData, (modifiedFlag & OcSituation::ModificationFlag::eDesignator) == OcSituation::ModificationFlag::eDesignator);
    bRet |= getSymbolSidc(uncompressed, pos, participantData, (modifiedFlag & OcSituation::ModificationFlag::eSymbol) == OcSituation::ModificationFlag::eSymbol);
    //sanity
    bool bInsane = false;
    if (bRet && (participantData.partyId() <= 0 || participantData.id() <= 0 || participantData.typeId() <= 0))
      bInsane = true;

    if (bRet && !bInsane)
    {
      int unitId = Settings::unitOffset() + participantData.partyId();
      if (participantData.id() == 0) // if new participant
      {
        participantData.id(participantId);
        participantData.modId(modId++);
        participantData.hierarchyNumber(participantId); // it just needs to be unqiue here
      }
      //always set superiorModId! changeing party Id only won't work
      participantData.superiorModId(unitId);

      participantData.assignState(ParticipantManagement::ParticipantAssignState::HandoutExcon); // set always as we can deduce it anyway //TODO cant reasign EQ nr

      if (!m_participantData.contains(unitId)) // Adding Unit to participantData, if not already added
      {
        m_participantData.insert(unitId, m_ppmDtc->convertParticipantToParticipantData(m_participants.value(unitId)));
        DEBUG_LOG(m_logger) << "Adding unit to m_participantData: " << m_participantData.value(unitId);
      }

      m_participantData.insert(participantId, participantData);
      DEBUG_LOG(m_logger) << "Adding ParticipantData to internal map: " << participantData;

      if (participantData.equipmentNumber().is_set())
        m_participantId2EqId[participantId] = participantData.equipmentNumber().get();
      else
        m_participantId2EqId[participantId] = 0;  // so the positions can be removed, see e.g. removeAllPositions()
    }

    // no position update for ourself. Easier to filter here than in server but a bit overhead in TETRA
    if (m_tabocDtc->specificsEquipment(1).equipmentNumber() == m_participantId2EqId.value(participantId))
      m_equipmentPositions.remove(participantId);
  }

  DEBUG_LOG(m_logger) << " m_equipmentPositions size after decode: " << m_equipmentPositions.size()
                      << " m_equipmentStates size after decode: " << m_equipmentStates.size()
                      << " m_equipmentOperationalStates size after decode: " << m_equipmentOperationalStates.size()
                      << " m_specialSensorData size after decode: " << m_specialSensorData.size();

  return true;
}

void HandlerOcSituationData::sendParticipantData(int requestId)
{
  if (m_modifiyRequests.contains(requestId))
  {
    WARNING_LOG(m_logger) << "Resending modfiyParticipantDataRequest: " << requestId;
    m_ppmFtc->modifyParticipantDataRequest(*m_modifiyRequests.value(requestId), this);
    return;
  }

  ParticipantManagement::ModifyParticipantDataRequest *newRequest = new ParticipantManagement::ModifyParticipantDataRequest();
  newRequest->requestId(requestId);
  newRequest->data(LtsUtils::convert<std::vector<ParticipantManagement::ParticipantData>>(m_participantData));
  newRequest->forceAddWithId(true);
  m_modifiyRequests.insert(requestId, newRequest);
  m_participantData.clear();

  INFO_LOG(m_logger) << "Sending modfiyParticipantDataRequest: " << requestId;
  m_ppmFtc->modifyParticipantDataRequest(*newRequest, this);
}

template <typename T>
void HandlerOcSituationData::publishMap(QMap<int, T> &map)
{
  for (auto it = map.begin(); it != map.end();)
  {
    if (m_participants.value(it.key()).id() > 0) // publishing only for existing participants
    {
      auto topic = it.value();
      topic.equipmentNumber(m_participantId2EqId.value(it.key()));
      m_tabDts->publish(topic);
      it = map.erase(it);
    }
    else
      ++it;
  }
}

void HandlerOcSituationData::sendTrainingAreaBaseData()
{
  publishMap<TrainingAreaBase::EquipmentPosition>(m_equipmentPositions);
  publishMap<TrainingAreaBase::EquipmentState>(m_equipmentStates);
  publishMap<TrainingAreaBase::EquipmentOperationalState>(m_equipmentOperationalStates);
  publishMap<TrainingAreaBase::SpecialSensorData>(m_specialSensorData);
  publishMap<TrainingAreaBase::ParticipantSanitation>(m_participantSanitations);
}

bool HandlerOcSituationData::getParticipantId(const QByteArray &buf, size_t &pos, int &participantId, OcSituation::ModificationBits &modifiedFlags) const
{
  bool bRet = true;
  bRet &= LtsUtils::get2Byte(buf, pos, participantId);
  bRet &= LtsUtils::get1Byte(buf, pos, modifiedFlags);
  return bRet;
}

bool HandlerOcSituationData::getPosition2D(const QByteArray &buf, size_t &pos, TrainingAreaBase::EquipmentPosition &position, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // lon, lat, sampleTime
  double tmp;
  bRet &= LtsUtils::get8Byte(buf, pos, tmp);
  position.position().position().lon(tmp);
  bRet &= LtsUtils::get8Byte(buf, pos, tmp);
  position.position().position().lat(tmp);
  position.sampleTime(m_dds->currentPartitionTime());

  return bRet;
}

bool HandlerOcSituationData::getPosition3D(const QByteArray &buf, size_t &pos, TrainingAreaBase::EquipmentPosition &position, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // lon, lat, sampleTime
  bRet &= getPosition2D(buf, pos, position, bAvailable);
  // z
  double tmp;
  int roomId = -1;
  int buildingId = -1;

  bRet &= LtsUtils::get8Byte(buf, pos, tmp);
  position.position().position().z(tmp);
  bRet &= LtsUtils::get2Byte(buf, pos, buildingId);
  bRet &= LtsUtils::get4Byte(buf, pos, roomId);

  if (buildingId > 0 && roomId > 0)
  {
    position.position().moutRoomId().set(roomId);
    position.position().moutObjectId().set(buildingId);
    position.position().reportingSource() = (tmp >= 0.0) ? Core::PositionReportingSource::Ops : Core::PositionReportingSource::OpsTunnel;
  }



  return bRet;
}

bool HandlerOcSituationData::getVBG(const QByteArray &buf, size_t &pos, TrainingAreaBase::EquipmentState &equipmentState, TrainingAreaBase::ParticipantSanitation &sanitation, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // vulnerabilityState, vulnerabilityBits, originatorParty, sampleTime
  uint64_t tmp;
  bRet &= LtsUtils::get1Byte(buf, pos, tmp);
  equipmentState.vulnerabilityState(ProjectDataManagement::VulnerabilityState::inner_enum(tmp));
  bRet &= LtsUtils::get4Byte(buf, pos, tmp);
  equipmentState.vulnerabilityKindId(tmp);
  bRet &= LtsUtils::get2Byte(buf, pos, tmp);
  equipmentState.vulnerabilityBits(tmp);
  bRet &= LtsUtils::get1Byte(buf, pos, tmp);
  if (!m_knownParties.contains(tmp))
  {

    WARNING_LOG(m_logger) << "Party: " << tmp << " unknown, setting equipmentState.originatorId() to "
                          << Settings::unitOffset() + 1;
    //Sollte nicht passieren, doch falls ja gibt es goldene Kreuze beim Ausfall
    tmp = 1;
  }
  equipmentState.originatorId(Settings::unitOffset() + tmp);

  if (m_bWithVBGTime)
  {
    bRet &= LtsUtils::get4Byte(buf, pos, tmp);
    equipmentState.sampleTime(tmp * 1000UL);
  }
  else
    equipmentState.sampleTime(QDateTime::currentMSecsSinceEpoch());

  quint8 vitRest;
  bRet &= LtsUtils::get1Byte(buf, pos, vitRest);
  if (bRet)
  {
    int vit = (vitRest & 0x0F) * 10;
    equipmentState.vitality(vit);
    TrainingAreaBase::SanitationResult result =  static_cast<TrainingAreaBase::SanitationResult_def::type>(vitRest >> 4);
    sanitation.sanitationResult(result);
    sanitation.proceduresTaken(1);
    sanitation.qualification(TrainingAreaBase::Qualification::SANPers);

  }
  qint8 originator = 0;
  bRet &= LtsUtils::get1Byte(buf, pos, originator);
  if (bRet)
    equipmentState.originatorType(ProjectDataManagement::StateChangeOriginator::inner_enum(originator));

  return bRet;
}

bool HandlerOcSituationData::getState(const QByteArray &buf, size_t &pos, TrainingAreaBase::SpecialSensorData &specialSensorData, TrainingAreaBase::EquipmentOperationalState &equipmentOperationalState, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // operationBits
  int tmp;
  bRet &= LtsUtils::get1Byte(buf, pos, tmp);

  specialSensorData.sampleTime(m_dds->currentPartitionTime());

  equipmentOperationalState.nbcProtectionActive((tmp & 0x02) || (tmp & 0x04));
  equipmentOperationalState.isManipulation(tmp & 0x02);

  equipmentOperationalState.sampleTime(m_dds->currentPartitionTime());

  return bRet;
}

bool HandlerOcSituationData::getStructure(const QByteArray &buf, size_t &pos, ParticipantManagement::ParticipantData &participant, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // equipmentNumber, typeId, partyId
  int tmp;
  bRet &= LtsUtils::get2Byte(buf, pos, tmp);
  participant.equipmentNumber(dds::core::optional<int32_t>(tmp));
  bRet &= LtsUtils::get2Byte(buf, pos, tmp);
  participant.typeId(tmp);
  bRet &= LtsUtils::get2Byte(buf, pos, tmp);
  if (m_knownParties.contains(tmp))
    participant.partyId(tmp);
  else
  {
    WARNING_LOG(m_logger) << "Not setting new party: " << tmp << " for participant: " << participant.id() << " as the party is unknown -> do a base download.";
    bRet = false;
  }

#ifndef GUEZ_MODE
  bRet &= LtsUtils::get2Byte(buf, pos, tmp);
  if (m_participants.contains(tmp))
    participant.superiorModId(tmp);
  else
    WARNING_LOG(m_logger) << "Not setting new superioir ID: " << tmp << " for participant: " << participant.id() << " as the superior is unknown -> do a base download.";
#endif

  return bRet;
}

bool HandlerOcSituationData::getDesignator(const QByteArray &buf, size_t &pos, ParticipantManagement::ParticipantData &participant, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  // designator, tacticalIdentifier
  int tmp;
  bRet &= LtsUtils::get1Byte(buf, pos, tmp);
  size_t len1 = tmp & 0x0F;
  size_t len2 = (tmp & 0xF0) >> 4;
  //hier wurde direkt auf dem Buffer zugegriffen ohne vorher zu prüfen ob es überhaupt passt
  if ((size_t)buf.size() < pos + len1)
    bRet = false;
  if (bRet)
    participant.designator(buf.mid(pos, len1).toStdString());
  pos += len1;
  if ((size_t)buf.size() < pos + len2)
    bRet = false;
  if (bRet)
    participant.tacticalIdentifier(buf.mid(pos, len2).toStdString());
  pos += len2;

  return bRet;
}

bool HandlerOcSituationData::getSymbolSidc(const QByteArray &buf, size_t &pos, ParticipantManagement::ParticipantData &participant, bool bAvailable) const
{
  if (!bAvailable)
    return false;
  bool bRet = true;

  size_t len;
  bRet &= LtsUtils::get1Byte(buf, pos, len);
  if (len == 0)
  {
    // get symbol from type id
    const auto type = m_ppmDtc->type(participant.typeId());
    participant.symbol(type.symbol());
    return bRet;
  }
  if ((size_t)buf.size() < pos + len)
    bRet = false;
  if (bRet)
    participant.symbol(buf.mid(pos, len).toStdString());
  pos += len;

  return bRet;
}

void HandlerOcSituationData::onPartyChanged(const ParticipantManagement::Party &party, DataModification mod)
{
  switch (mod)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
    m_knownParties.insert(party.id());
    break;
  case DataModification::eRemoved:
    m_knownParties.remove(party.id());
    break;
  }
}

void HandlerOcSituationData::onParticipantChanged(const ParticipantManagement::Participant &participant, DataModification mod)
{
  switch (mod)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
    m_participants[participant.id()] = participant;
    // if there is a participant but it has (currently) no equipno, remove it from sitmap by setting invalid position
    if (!m_participantId2EqId.contains(participant.id()))
      setInvalidPositionForParticipant(participant.id());
    break;
  case DataModification::eRemoved:
    m_participants.remove(participant.id());
    break;
  }
}


} // namespace OcSituationClient