Initial commit

Lts/Services/ParticipantManagement/UasHandling/mission_plan_handler.cpp

@@ -0,0 +1,444 @@
+/**
+  * The UasHandler class manages all Uas related function topics and holds classes for
+  * further Uas data.
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by Steve Kemayou, ESZS, 2025-06-02
+  */
+
+#include <ngldb/db_service_handler.h>
+#include <ngldb/dbtransaction.h>
+#include "main_handler.h"
+#include "ParticipantHandling/participant.h"
+
+#include "mission_plan_handler.h"
+
+namespace ParticipantManagement
+{
+namespace UasHandling
+{
+MissionPlanHandler::MissionPlanHandler(MainHandler *mainHandler)
+  : Interface::Fts::ModifyMissionPlanRequestHandler(mainHandler->fts())
+  , Interface::Fts::RemoveMissionPlanRequestHandler(mainHandler->fts())
+  , Interface::Fts::SetMissionActionFulfilledRequestHandler(mainHandler->fts())
+  , m_mainHandler(mainHandler)
+  , m_logger(getLogger("MissionPlanHandler"))
+{
+
+}
+
+ParticipantManagement::UasHandling::MissionPlanHandler::~MissionPlanHandler()
+{
+  qDeleteAll(m_objMissionPlans);
+}
+
+bool MissionPlanHandler::init()
+{
+  DEBUG_LOG(m_logger)<<" -- MissionPlanHandler init...";
+  qDeleteAll(m_objMissionPlans);
+  m_objMissionPlans.clear();
+  if (!loadMissionPlansFromDb())
+    return false;
+  return true;
+}
+
+void MissionPlanHandler::onModifyMissionPlanRequest(const rti::core::SampleIdentity &cookie, const ModifyMissionPlanRequest &request)
+{
+  Core::FunctionReplyObject reply = request.id() == 0 ? createMissionPlanRequest(request)
+                                                      : modifyMissionPlanRequest(request);
+  m_mainHandler->fts()->modifyMissionPlanReply(cookie, reply);
+}
+
+void MissionPlanHandler::onRemoveMissionPlanRequest(const rti::core::SampleIdentity &cookie, const RemoveMissionPlanRequest &request)
+{
+  Core::FunctionReplyObject reply = removeMissionPlanRequest(request.id());
+
+  m_mainHandler->ddsHandler()->fts()->removeMissionPlanReply(cookie, reply);
+}
+
+void MissionPlanHandler::onSetMissionActionFulfilledRequest(const rti::core::SampleIdentity &cookie, const SetMissionActionFulfilledRequest &request)
+{
+  //DEBUG_LOG(m_logger)<<">> -- onSetMissionActionFulfilledRequest -- ";
+  Core::FunctionReplyObject reply = setMissionActionFulfilledRequest(request);
+  m_mainHandler->ddsHandler()->fts()->setMissionActionFulfilledReply(cookie, reply);
+}
+
+bool MissionPlanHandler::loadMissionPlansFromDb()
+{
+  QVector<ParticipantManagement::Database::PpmMissionPlan> missionPlanVec;
+  QVector<ParticipantManagement::Database::PpmMissionItem> missionItemVec;
+  QVector<ParticipantManagement::Database::PpmMissionAction> missionActionVec;
+  if (!m_mainHandler->db()->select(missionActionVec))
+  {
+    ERROR_LOG(m_logger)<<"Error reading data from database >> PpmMissionAction: "<<missionActionVec.size();
+    return false;
+  }
+
+  if (!m_mainHandler->db()->select(missionItemVec))
+  {
+    ERROR_LOG(m_logger)<<"Error reading data from database >> missionItemVec"<<missionItemVec.size();
+    return false;
+  }
+
+  if (!m_mainHandler->db()->select(missionPlanVec))
+  {
+    ERROR_LOG(m_logger)<<"Error reading data from database >> missionPlanVec"<<missionPlanVec.size();
+    return false;
+  }
+  DEBUG_LOG(m_logger) << "load MissionPlans From Db  Plans size = " << missionPlanVec.size()
+                      <<" Items size = "<<missionItemVec.size()
+                      <<" Actions size = "<<missionActionVec.size();
+  // add to internal buffer
+  for (const ParticipantManagement::Database::PpmMissionPlan &data : qAsConst(missionPlanVec))
+  {
+    ParticipantManagement::MissionPlan missionPlan;
+    missionPlan.id(data.id);
+    missionPlan.name(data.name.toStdString());
+    missionPlan.participantId(data.participantId);
+    missionPlan.missionStart(data.missionStart.toMSecsSinceEpoch());
+    missionPlan.repeat(data.repeat);
+    missionPlan.jumpToStart(data.jumpToStart);
+
+
+    QMap <int, ParticipantManagement::Database::PpmMissionItem> itemsDbMap; // QMap to get it sorted
+    QMultiMap <int, ParticipantManagement::Database::PpmMissionAction> actionsDbMap;
+    //get mission items and actions for curr mission plan id
+
+    for (const ParticipantManagement::Database::PpmMissionItem &itemDb : qAsConst(missionItemVec))
+    {
+      if (itemDb.missionPlanId == data.id)
+      {
+        itemsDbMap.insert(itemDb.elementNumber, itemDb);
+        for (const ParticipantManagement::Database::PpmMissionAction &actionDb : qAsConst(missionActionVec) )
+        {
+          if (actionDb.missionItemId == itemDb.id)
+            actionsDbMap.insert(actionDb.missionItemId, actionDb);
+        }
+      }
+    }
+
+
+    missionPlan.items().reserve(itemsDbMap.size());
+    for (const ParticipantManagement::Database::PpmMissionItem &item : qAsConst(itemsDbMap))
+    {
+      ParticipantManagement::MissionItem msi;
+      msi.actions().reserve(actionsDbMap.size());
+      msi.name(item.name.toStdString());
+      msi.location(Core::GeoPosition(item.location.x(), item.location.y(), item.location.z()));
+      msi.itemNumber(item.elementNumber);
+      int actionsFulfilled = 0;
+      int64_t latestFulfilled = 0;
+      for (const auto &action : actionsDbMap)
+      {
+        if (action.missionItemId == item.id)
+        {
+          ParticipantManagement::MissionAction msa;
+          msa.planId(missionPlan.id());
+          msa.actionType(action.missionActionTypeId);
+          msa.duration(action.duration);
+          msa.fulfilled(action.fulfilled.toMSecsSinceEpoch());
+          msa.properties(action.properties.toStdString());
+          msa.name(action.name.toStdString());
+          msa.actionNumber(action.elementNumber);
+          msa.itemNumber(msi.itemNumber());
+          msi.actions().push_back(msa);
+
+          if (msa.fulfilled() > 0)
+            actionsFulfilled++;
+
+          if (latestFulfilled < msa.fulfilled())
+            latestFulfilled = msa.fulfilled();
+
+        }
+      }
+
+      msi.actionsFulfilled(actionsFulfilled);
+      msi.fulfilled(latestFulfilled);
+
+      missionPlan.items().push_back(msi);
+    }
+
+    setLatestFulfilledItem(missionPlan); // update mission plan fulfillment count & fulfillemnt latest time
+
+    ObjectMissionPlan *obj = new ObjectMissionPlan(m_mainHandler->ddsHandler(), m_mainHandler->db(), missionPlan);
+    obj->setDbMissionItems(itemsDbMap, actionsDbMap);
+    m_objMissionPlans.insert(missionPlan.id(), obj);
+
+    // publish mission plan data
+    m_mainHandler->ddsHandler()->dts()->publish(missionPlan);
+
+  }
+  DEBUG_LOG(m_logger)<<" -- END load mission plan From Db -- ";
+  return true;
+}
+
+bool MissionPlanHandler::isUasInAutoMode(int participantId)
+{
+  m_mainHandler->ddsHandler()->dtc()->waitForHistoricalDataUAS(3000);
+  const auto uas = m_mainHandler->ddsHandler()->dtc()->getUasByParticipantId(participantId, m_mainHandler->exerciseId());
+
+  if (uas.id() < 1)
+  {
+    ERROR_LOG(m_logger) << "no uas for pid " << participantId;
+    return false;
+  }
+
+  DEBUG_LOG(m_logger) << "uas with pid " << participantId << " is in mode: " << (int)uas.uasState().underlying();
+
+  return (uas.uasState() == ProjectDataManagement::UasState::Auto);
+}
+
+// stolen from Lts/Plugins/ParticipantManagement/Gui/ParticipantInfo/participant_info_dialog.cpp
+bool MissionPlanHandler::isUasCombatReady(int participantId)
+{
+  m_mainHandler->ddsHandler()->dtc()->waitForHistoricalDataDynamicParticipantData(3000);
+  const auto data = m_mainHandler->ddsHandler()->dtc()->dynamicParticipantData(participantId);
+
+  if (data.participantId() == 0)
+  {
+    return false;
+  }
+
+  //Current State
+  if (data.stateHistory().empty())
+  {
+    return false;
+  }
+
+  const auto &history = data.stateHistory();
+
+  //LasState is the last "real" state, that has a VBG <=8 (Castastrophic)
+  //It's not the last state received, as there can be other state changes like Sanitation, or NearMiss
+  auto lastState = m_mainHandler->ddsHandler()->dtc()->youngestStateInformation(history/*, true*/ /* debug */);
+  decltype(lastState) youngestState;
+  youngestState.stateTime(0);
+  for (const auto &si : history)
+  {
+    DEBUG_LOG(m_logger) << "StateInformation "
+                         << " time=" << si.stateTime()
+                         << " vulnerabilityBits=" << si.vulnerabilityBits()
+                         << " vulnerabilityKindId=" << si.vulnerabilityKindId();
+    if (youngestState.stateTime() < si.stateTime())
+      youngestState = si;
+  }
+
+  return (youngestState.vulnerabilityState() == ProjectDataManagement::VulnerabilityState::CombatReady);
+}
+
+Core::FunctionReplyObject MissionPlanHandler::createMissionPlanRequest(const ModifyMissionPlanRequest &request)
+{
+  DEBUG_LOG(m_logger) <<" >> -- createMissionPlanRequest -- ";
+  if (request.participantId() > 0 && !isUasParticipant(request.participantId()))
+  {
+    return Core::FunctionReplyObject(tr_lts("dds", "Participant with id %1 is not a Uas participant.")).arg(request.participantId());
+  }
+
+  if (!isUasInAutoMode(request.participantId()))
+  {
+    return Core::FunctionReplyObject(tr_lts("dds", "Uas with participant id %1 is not in auto mode - maybe the safety tablet is currently engaged.")).arg(request.participantId());
+  }
+
+  if (!isUasCombatReady(request.participantId()))
+  {
+    return Core::FunctionReplyObject(tr_lts("dds", "Uas with participant id %1 ------ not combat ready.")).arg(request.participantId());
+  }
+
+  /// 1- insert in DB Mission plan
+  /// 2- insert in DB Mission Items
+  /// 3- insert in DB Mission Actions
+
+  //1-) insert mission plan
+  Database::PpmMissionPlan dbMissionPlan = ObjectMissionPlan::toDbMissionPlan(request);
+  const MissionPlan missionPlan =  ObjectMissionPlan::fillMissionPlanData(request);
+
+  ObjectMissionPlan *obj = new ObjectMissionPlan(m_mainHandler->ddsHandler(), m_mainHandler->db(), missionPlan);
+  Core::FunctionReplyObject ret = obj->createMissionPlan(dbMissionPlan, request.items());
+  if (ret.error())
+  {
+    delete obj;
+    return ret;
+  }
+  m_objMissionPlans.insert(obj->id(), obj);
+
+  publishMissionPlan(obj->getMissionPlanData()); // publish mission plan
+  return Core::FunctionReplyObject(dbMissionPlan.id);
+}
+
+Core::FunctionReplyObject MissionPlanHandler::modifyMissionPlanRequest(const ModifyMissionPlanRequest &request)
+{
+  DEBUG_LOG(m_logger)<<" >> -- modifyMissionPlanRequest -- ";
+  /// 1- update Mission plan Table
+  /// 2- remove first all mission items and their actions
+  /// 3- add New mission items
+  /// 4- add new mission actions
+  ObjectMissionPlan *obj = m_objMissionPlans.value(request.id());
+  if (!obj || request.id() < 0)
+    return Core::FunctionReplyObject(request.id(), tr_lts("dds", "Couldn't remove Mission plan with id %1")).arg(request.id());
+
+  const Database::PpmMissionPlan &dbMissionPlan = ObjectMissionPlan::toDbMissionPlan(request);
+
+  auto ret = obj->updateMissionPlan(dbMissionPlan, request.items());
+  publishMissionPlan(obj->getMissionPlanData()); // publish mission plan
+  return ret;
+}
+
+Core::FunctionReplyObject MissionPlanHandler::removeMissionPlanRequest(int id)
+{
+  DEBUG_LOG(m_logger)<<" >> -- removeMissionPlanRequest -- ";
+  Core::FunctionReplyObject ret;
+  ObjectMissionPlan *obj = m_objMissionPlans.value(id, nullptr);
+  if (!obj)
+    return ret;
+  ret = obj->removeMissionPlan(id);
+  if (ret.error())
+    return ret;
+  delete obj;
+  m_objMissionPlans.remove(id);
+
+  m_mainHandler->ddsHandler()->dts()->removeMissionPlanByKey(id);
+  return ret;
+}
+
+
+bool MissionPlanHandler::setCurrentMissionActionFulfilled(int participantId, const QString &nameOfTheMissionThatEnded)
+{
+  // okay we somehow need to find the currently running mission for this participant..... the plan: filter mission plans by participant und by (start < now)
+  bool found = false;
+  for (const auto &missionPlan : m_objMissionPlans.values())
+  {
+    if (missionPlan->missionPlan().participantId() == participantId && missionPlan->missionPlan().missionStart() <= m_mainHandler->currentTimeMSec())
+    {
+      // get current action (stolen from Services/CentralEffectSimulation/Plugins/MissionPlan/ces_mission_plan_object.cpp)
+      const std::vector< ParticipantManagement::MissionItem > items = missionPlan->missionPlan().items(); // copy !!
+      for (const auto &item : items)
+      {
+        for (auto &msa : item.actions())
+        {
+          const QString thisMissionName(QStringLiteral("%1_%2_%3").arg(missionPlan->missionPlan().id()).arg(QString::fromStdString(missionPlan->missionPlan().name()), QString::fromStdString(item.name())));
+
+          INFO_LOG(m_logger) << ">> mission item: this mission name: " << thisMissionName << ", fulfilled: " << msa.fulfilled();
+
+          if (msa.fulfilled() > 0)
+          {
+            continue;
+          }
+
+          if (thisMissionName != nameOfTheMissionThatEnded)
+          {
+            continue;
+          }
+
+          DEBUG_LOG(m_logger) << ">> ######### setMissionActionFulfilledRequest !!! item = " << item.itemNumber() << ", name = " << item.name() << ", missionPlan = " << missionPlan->missionPlan().id();
+          SetMissionActionFulfilledRequest req;
+          req.actionNumber(msa.actionNumber());
+          req.fulfilled(true);
+          req.missionId(missionPlan->missionPlan().id());
+          req.itemNumber(item.itemNumber());
+          const auto res = setMissionActionFulfilledRequest(req);
+          if (res.error())
+          {
+            ERROR_LOG(m_logger) << "failed to set current mission action as fulfilled: " << res.str();
+          }
+
+          found = true;
+          break;
+        }
+
+        if (found)
+        {
+          break;
+        }
+      }
+
+      if (found)
+      {
+        break;
+      }
+    }
+  }
+
+  if (found)
+  {
+    return true;
+  }
+
+  // maybe okay? last mission action was already fulfilled? is this even possible at this point?
+  DEBUG_LOG(m_logger) << "failed to set current mission action as fulfilled: no action found for pId " << participantId;
+  return false;
+}
+
+Core::FunctionReplyObject MissionPlanHandler::setMissionActionFulfilledRequest(const SetMissionActionFulfilledRequest &request)
+{
+  DEBUG_LOG(m_logger)<<" -- [setMissionActionFulfilledRequest] req.missionId: "<<request.missionId();
+  Core::FunctionReplyObject ret;
+  ObjectMissionPlan *obj = m_objMissionPlans.value(request.missionId(), nullptr);
+  if (!obj)
+    return ret;
+  ret = obj->setMissionActionFulfilled(request, m_mainHandler->currentTimeMSec());
+  m_mainHandler->ddsHandler()->dts()->publish(obj->missionPlan());
+  return ret;
+}
+
+void MissionPlanHandler::publishMissionPlan(const MissionPlan &missionplan)
+{
+  m_mainHandler->ddsHandler()->dts()->publish(missionplan);
+}
+
+bool MissionPlanHandler::isUasParticipant(int participantId) const
+{
+  const ParticipantManagement::ParticipantHandling::Participant *PARTICIPANT = m_mainHandler->participantHandler()->participantById(participantId);
+
+  if (PARTICIPANT == nullptr)
+  {
+    WARNING_LOG(m_logger) << "Got invalid participantId " << participantId;
+    return false;
+  }
+
+  const int TYPE_ID = PARTICIPANT->typeId();
+  return  m_mainHandler->ddsHandler()->dtc()->isTypeUas(TYPE_ID);
+}
+
+int MissionPlanHandler::getNumberOfActionFulfilled(const MissionItem &msi) const
+{
+  int count = 0;
+  for (auto action : msi.actions())
+  {
+    if (action.fulfilled() > 0)
+      count++;
+  }
+  return count;
+}
+
+void MissionPlanHandler::setLatestFulfilledItem(MissionPlan &missionPlan)
+{
+  int count = 0;
+  int64_t latestTime = 0;
+  for (auto item : missionPlan.items())
+  {
+    if (item.actionsFulfilled() > 0)
+      count++;
+
+    if (latestTime < item.fulfilled())
+      latestTime = item.fulfilled();
+
+    missionPlan.itemsFulfilled(count);
+    missionPlan.fulfilled(latestTime);
+  }
+}
+
+int64_t MissionPlanHandler::getLastestFulfilledAction(const MissionItem &msi) const
+{
+  int64_t latestTime = 0;
+  for (auto action : msi.actions())
+  {
+    if (action.fulfilled() > latestTime)
+      latestTime = action.fulfilled();
+  }
+
+  return latestTime;
+}
+
+
+}// namespace UasHandling
+}