/**
  * Plugin CES Mission Plan
  * $Id:  $
  * $HeadURL: svn://slssvn/TDS/branches/product/features/25_03_etb_uas/Lts/Services/CentralEffectSimulation/Plugins/MissionPlan/ces_mission_plan_handler.cpp $
  * $Author: Kemayou_s $
  * Created by Steve Kemayou, ESZS, 2025-06-25
  */

#include "ces_mission_plan_handler.h"

#include <QTimer>
#include <QDateTime>
#include <QRandomGenerator>

#include <LtsUtilsCore/dds_bitarray.h>
#include <LtsCoordTrans/lts_coord_trans.h>
#include <ProjectDataManagement/pdm_enums.hpp>

namespace dds
{
namespace core
{
inline uint qHash(ProjectDataManagement::EventType et)
{
  return ::qHash(et.underlying());
}
}
}

namespace CES_MISSIONPLAN
{


CesMissionPlanHandler::CesMissionPlanHandler(const std::shared_ptr<CentralEffectSimulation::DdsHandler> &ddsHandler,
                                               const std::shared_ptr<CentralEffectSimulation::HandlerParticipant> &participantHandler,
                                               QObject *parent)
  : QObject(parent)
  , m_ddsHandler(ddsHandler)
  , m_handlerParticipant(participantHandler)
  , m_logger(CentralEffectSimulation::getLogger("Plugin.MissionPlan.CesMissionPlanHandler"))
{
} // end of ctor()
//-----------------------------------------------------------------------------

CesMissionPlanHandler::~CesMissionPlanHandler()
{
  qDeleteAll(m_missionPlanObjects);
} // end of dtor()
//-----------------------------------------------------------------------------

bool CesMissionPlanHandler::init()
{
  // TAB FTC
  m_ddsHandler->dtcPpm()->enableUAS();
  m_ddsHandler->dtcPpm()->enableMissionPlan();
  m_ddsHandler->dtcPpm()->enableParticipantGearEvent();
  m_ddsHandler->ftcPpm()->enableSetMissionActionFulfilled();
  m_ddsHandler->ftcAes()->enableExplosionClusterModify();
  m_ddsHandler->ftcTab()->enableSendUasWaypoint();

  connect (m_ddsHandler->dtcPpm().get(), &ParticipantManagement::Interface::Dtc::uASChanged,
           this, &CesMissionPlanHandler::onUasChanged);

  connect (m_ddsHandler->dtcPpm().get(), &ParticipantManagement::Interface::Dtc::missionPlanChanged,
           this, &CesMissionPlanHandler::onMissionPlanChanged);


  DEBUG_LOG(m_logger) << " -- MissionPlanHandler::init() finished.";
  return true;
}

void CesMissionPlanHandler::updateMissionPlanList(const ParticipantManagement::MissionPlan &data)
{
  CesMissionPlanObject *obj = m_missionPlanObjects.value(data.id(), nullptr);

  if (obj)
  {
    m_missionPlanObjects[data.id()]->setMissionPlan(data);
    return;
  }

  // check if mission start is in the future before proceeding..
  DEBUG_LOG(m_logger)<<" -- [updateMissionPlanList] new missionPlan : "<<data.name()<< " planId: "<<data.id();

  if (data.missionStart() < m_ddsHandler->currentPartitionTime())
  {
    WARNING_LOG(m_logger) << " -- updateMissionPlanList mission startTime ("<<data.missionStart()<<") in the past ignore mission plan id: "<<data.id()
                          << " name: " << data.name() << " - now: " << m_ddsHandler->currentPartitionTime();
    return;
  }

  obj = new CesMissionPlanObject(data, m_ddsHandler, this);
  connect(obj, &CesMissionPlanObject::ready2SendNextAction, this,
          &CesMissionPlanHandler::onSendNextMissionAction, Qt::UniqueConnection);

  const int planId = obj->planId();
  std::function<void(void)> onDeleteObj = [this, planId]()
  {
    DEBUG_LOG(m_logger)<<" --  remove ces mission object from container planId: "<<planId;
    m_missionPlanObjects.remove(planId);
  };
  connect(obj, &CesMissionPlanObject::destroyed, this, onDeleteObj);

  m_missionPlanObjects.insert(data.id(), obj);
}

void CesMissionPlanHandler::onUasChanged(const ParticipantManagement::UAS &data, DataModification modType)
{
  Q_UNUSED(modType)
  DEBUG_LOG(m_logger)<<" -- onUasChanged : uas.id = "<<data.id() <<" participantId = "<<data.participantId();
  // get mission object of corresponding uas participant
  // if drone status is manual mode then cancel mission.
  if (data.uasState() != ProjectDataManagement::UasState_def::Manual && modType != DataModification::eRemoved)
    return;

  for (auto planObj :  m_missionPlanObjects)
  {
    if (planObj->participantId() != data.participantId())
      continue;

    // if waypoint started and state switched to manual abord mission
    if (planObj->missionPlan().missionStart() < m_ddsHandler->currentPartitionTime())
      planObj->abortMission();
    break;
  }
}

void CesMissionPlanHandler::onMissionPlanChanged(const ParticipantManagement::MissionPlan &data, DataModification modType)
{
  DEBUG_LOG(m_logger)<<" -- onMissionPlanChanged : missionPlanName: "<<data.name() << ", modType: " << (int)modType;
  if (data.id() <= 0)
    return;
  switch (modType)
  {
  case DataModification::eAdded:
  case DataModification::eModified:
    updateMissionPlanList(data);
    break;
  case DataModification::eRemoved:
  {
    auto obj = m_missionPlanObjects.value(data.id(), nullptr);
    if (!obj)
      return;
    delete obj;

    m_missionPlanObjects.remove(data.id());
  }
    break;
  default:
    break;
  }
}

bool CesMissionPlanHandler::isUasInAutoMode(int participantId)
{

  m_ddsHandler->dtcPpm()->waitForHistoricalDataUAS(3000);
  const auto uas = m_ddsHandler->dtcPpm()->getUasByParticipantId(participantId, m_ddsHandler->getExerciseId());

  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 CesMissionPlanHandler::isUasCombatReady(int participantId)
{

  m_ddsHandler->dtcPpm()->waitForHistoricalDataDynamicParticipantData(3000);
  const auto data = m_ddsHandler->dtcPpm()->dynamicParticipantData(participantId);

  if (data.participantId() == 0)
  {
    return false;
  }

  //Current State
  DEBUG_LOG(m_logger) << "stateHistory count" << data.stateHistory().size();
  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_ddsHandler->dtcPpm()->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);
}

void CesMissionPlanHandler::onSendNextMissionAction(const ParticipantManagement::MissionAction &msa)
{
  auto missionPlan = m_missionPlanObjects.value(msa.planId(), nullptr);
  if (!missionPlan)
  {
    DEBUG_LOG(m_logger)<<" -- [SEND NEXT MISSION] Object not found. PlanId: "<<msa.planId();
    return;
  }


  // send to TAB
  const auto participantId = missionPlan->m_missionPlan.participantId();
  const auto participant = m_handlerParticipant->participant(participantId);

  if (!participant.equipmentNumber())
  {
    ERROR_LOG(m_logger) << "unable to send next mission action to uav without equipment number";
    return;
  }




  if (!isUasInAutoMode(participantId))
  {
    ERROR_LOG(m_logger) << "unable to send next mission action to uav that is not in auto mode";
    //TODO: fulfill all other missions or mark as incomplete
    return;
  }
 

  if (!isUasCombatReady(participantId))
  {
    ERROR_LOG(m_logger) << "unable to send next mission action to uav that is not combat ready";
    //TODO: fulfill all other missions or mark as incomplete
    return;
  }

  const auto eqId = participant.equipmentNumber().value();

  try
  {
    // send action to uav via tab - if this doesn't throw, waypoint is now executing
    m_ddsHandler->ftcTab()->sendUasWaypointRequest(TrainingAreaBase::SendUasWaypointRequest(eqId, participantId, msa.actionNumber(), msa.itemNumber(), msa.planId(), msa.name(), msa.properties()));
    // PPM will send a change request to the current active action, which in return tell us (updateMissionPlanList) and finally we will start the next action - so we are done here for now
  }
  catch (const std::exception &ex)
  {
    ERROR_LOG(m_logger) << "unable to send next mission action to uav: " << ex.what();
    return;
  }
}


} // namespace CES_MISSIONPLAN