/**
  * Manages the uav interface instances (for TMS-IF) per uav participant
  * $Id$
  * $HeadURL$
  * $Author$
  *
  * Created by Mike Wille, ESZS, 2025-05-22
  */

#include "uav_ia_manager.h"
#include "ParticipantManagement/ppm_ftc.h"
#include "logger.h"

#include <LtsUtilsCore/json.h>
#include <LtsCoordTrans/lts_coord_trans.h>
#include <ParticipantManagement/ppm_dtc.h>
#include <TrainingAreaBase/recv_from_tab.h>
#include <TmsIfAdapter/tms_if_adapter.h>
#include <TmsIfAdapter/uav_commander.h>

#include <QVector3D>
#include <QTimer>

#include <vector>


namespace UAS
{
const int UAVInterfaceAdapterManager::s_domainId = 4;


UAVInterfaceAdapterManager::UAVInterfaceAdapterManager(const std::shared_ptr<TrainingAreaBase::DdsHandler> &ddsHandler,
                                                       const std::shared_ptr<TrainingAreaBase::RecvFromTab> &sendToDds,
                                                       TmsIfAdapter &tmsIfAdapter, int exerciseId, QObject *parent)
  : QObject(parent)
  , m_exerciseId(exerciseId)
  , m_dtcPpm(ParticipantManagement::Interface::Dtc::create(ddsHandler->ddsConnector()))
  , m_ftcPpm(ParticipantManagement::Interface::Ftc::create(ddsHandler->ddsConnector()))
  , m_tmsIfAdapter(tmsIfAdapter)
  , m_sendToDds(sendToDds)
  , m_logger(getLogger("UAVManager"))
{}


void UAVInterfaceAdapterManager::init()
{
  DEBUG_LOG(m_logger) << "UAVInterfaceAdapterManager starting for ex " << m_exerciseId;

  m_dtcPpm->waitForHistoricalDataDynamicParticipantData(3000);
  m_dtcPpm->waitForHistoricalDataParticipant(3000);
  m_dtcPpm->waitForHistoricalDataUAS(3000);
  m_dtcPpm->waitForHistoricalDataMissionPlan(3000);


  connect(m_dtcPpm.get(), &ParticipantManagement::Interface::Dtc::uASChanged,
          this, [this](const ParticipantManagement::UAS &p, DataModification mod)
  {
    switch (mod)
    {
      case DataModification::eRemoved:
      {
        removed(p);
        return;
      }

      case DataModification::eModified:
      {
        modified(p);
        return;
      }

      case DataModification::eAdded:
      {
        added(p);
        return;
      }
    }
  });

  for (const auto &p : m_dtcPpm->uASs())
  {
    added(p);
  }

  if (!m_dtcPpm->isUASEnabled())
  {
    m_dtcPpm->enableUAS();
  }

  DEBUG_LOG(m_logger) << "done";
}


bool UAVInterfaceAdapterManager::tryChangeModeToAuto(int participantId)
{
  return kill(participantId);
}


void UAVInterfaceAdapterManager::removed(const ParticipantManagement::UAS &uas)
{
  //There should be no active UAV but just in case
  if (m_uavs.find(uas.id()) != m_uavs.cend())
  {
    WARNING_LOG(m_logger) << "tried removing participant " << uas.id() << " that doesn't exist";
    return;
  }
  DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found! removing.....";
  stop(uas);
}


void UAVInterfaceAdapterManager::modified(const ParticipantManagement::UAS &uas)
{
  DEBUG_LOG(m_logger) << "modify uas " << uas.id();
  // if typeId has changed, it's not a UAV anymore, so stop and done
  if (uas.participantId() <= 0)
  {
    WARNING_LOG(m_logger) << "UAV with uas id " << uas.id() << " not a UAV anymore - removing";
    stop(uas); // remove
    return;
  }

  const UasId uasId = uas.id();

  // relying on dds working as intended and not doing anything if UAV doesn't exist

  const auto i = m_uavs.find(uasId);
  if (i == m_uavs.cend())
  {
    WARNING_LOG(m_logger) << "UAV with uas id " << uas.id() << " was modified but now found in our list - feeding it as 'added' instead as a bail-out";
    added(uas); // add, maybe UAV was linked to an participant again - note that if the UAV was unlinked, it would have been stopped some lines above (see uas.participantId check above)
    return;
  }

  const UAVInterfaceAdapter &uav2 = *i->second;

  DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found";

  try
  {
    UAVInterfaceAdapterConfig config(QString::fromStdString(uas.hostname()), uas.hostport(), uas.id(), uas.useTcp());

    if (uav2.config() == config)
    {
      DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " already running and config has not changed - doing nothing (irrelevant data has changed)";
      return;
    }

    // restart:
    stop(uas);
    QTimer::singleShot(5000, this, [this, uas, config] // wait until everything is stopped
    {
      start(uas, config);
    });
  }
  catch(const std::exception &ex)
  {
    WARNING_LOG(m_logger) << "Failed to modify UAV: " << ex.what();
  }
}


void UAVInterfaceAdapterManager::added(const ParticipantManagement::UAS &uas)
{
  DEBUG_LOG(m_logger) << "exId: " << m_exerciseId << ", uas: " << uas.id() << ", participantId: " << uas.participantId()<<" ## use tcp ? "<<uas.useTcp();

  const auto participant = m_dtcPpm->participant(uas.participantId());

  if (!participant.equipmentNumber())
  {
    return;
  }

  if (uas.exerciseId() != m_exerciseId)
  {
    return;
  }

  DEBUG_LOG(m_logger) << "added uav with id " << uas.id() << " for pId " << uas.participantId();

  if (m_uavs.find(uas.id()) != m_uavs.cend())
  {
    // assert(false);
    DEBUG_LOG(m_logger) << "participant " << uas.id() << " already started - doing nothing";
    return;
  }

  DEBUG_LOG(m_logger) << "############# ALLES OKAY uas: " << uas.id() << " - pId: " << uas.participantId();
  DEBUG_LOG(m_logger) << "UAV with uas id " << uas.id() << " found";

  try
  {
    UAVInterfaceAdapterConfig config(QString::fromStdString(uas.hostname()), uas.hostport(), uas.id(), uas.useTcp());
    start(uas, config);
  }
  catch (const std::exception &ex)
  {
    WARNING_LOG(m_logger) << "Failed to start uav ia: " << ex.what();
  }
}


void UAVInterfaceAdapterManager::start(const ParticipantManagement::UAS &uas, UAVInterfaceAdapterConfig config)
{
  try
  {
    const ParticipantId pId = uas.participantId();
    const auto participant = m_dtcPpm->participant(uas.participantId());

    if (participant.id() < 1 || !participant.equipmentNumber())
    {
      throw std::runtime_error(QStringLiteral("pid < 1 or eqid not set for pid %1").arg(pId).toStdString());
    }

    const auto uasId = uas.id();
    const auto droneEqId = participant.equipmentNumber().value();
    const auto droneParticipantId = uas.participantId();
    const int droneDomainId = s_domainId;
    const bool useTcp = uas.useTcp();

    DEBUG_LOG(m_logger) << "starting IA for eqId = " << droneEqId << ", pid = " << droneParticipantId << ", domain = " << droneDomainId << ", uasId = " << uasId<<"  // ** useTcp: "<<uas.useTcp();

    auto *uav = m_tmsIfAdapter.createUAV(droneDomainId, uasId); // Do weneed one drone per domain or n drones per domain but with different service names (instead of always 1) ?

    m_uavs[uasId] = new UAVInterfaceAdapter(/*std::move*/(config), uav); // starts tms if uav ia


    const auto fn = [this, droneDomainId, droneParticipantId, droneEqId, useTcp]()
    {
      DEBUG_LOG(m_logger) << "UAV " << droneDomainId << " / " << droneParticipantId <<"  / use TCP ? "<<useTcp<< " ready !!"; // << " ("<<(quintptr)this<<")";

      const auto pdata = m_dtcPpm->dynamicParticipantData(droneParticipantId);

      if (pdata.participantId() < 1 || pdata.stateHistory().empty()) // if a participant doesn't have dynamic pdata or a state history, that means it's not even combat ready or got whatever state - so set it on first ready
      {
        DEBUG_LOG(m_logger) << "pdata or state history for pid " << droneParticipantId << " is empty - sending init";

        TrainingAreaBase::TetraIcd::StateChange msg;
        msg.header.equipmentNumber = droneEqId;
        msg.header.timeStamp =  QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
        msg.originatorId = 0;
        msg.originatorType = TrainingAreaBase::TetraIcd::OriginatorType::Initialisation;
        msg.tasVitality = 100;
        msg.vulnerabilityBits = 0;
        msg.vulnerabilityState = TrainingAreaBase::TetraIcd::VulnerabilityState::CombatReady;

        m_sendToDds->rxData(msg);

        DEBUG_LOG(m_logger) << "sent combat ready for " << droneParticipantId << " - done";
      }
      else
      {
        WARNING_LOG(m_logger) << "state history for pid " << droneParticipantId << " not empty - nothing to do";
      }
    };
    uav->set_isReady_Callback(fn);


    uav->set_CoordinateFrame_ReportPosition_Callback([this, droneEqId](const CoordinateFrame::ReportPositions::Message &data)
    {
      // send drone position to DDS, to display the participant on the map

      TrainingAreaBase::TetraIcd::PositionReport msg;
      msg.header.equipmentNumber = droneEqId;
      msg.header.timeStamp =  QDateTime::currentMSecsSinceEpoch(); //data.timestamp * 1000;
      msg.geoPosition = { data.lon * LtsCoordTrans::deg2rad(), data.lat * LtsCoordTrans::deg2rad(), data.alt }; // convert lat/lon in deg (uav format) to lat/lon in rad (excon format)

      DEBUG_LOG(m_logger) << "POSITION! sending PositionReport into TAB"; // << " ("<<(quintptr)this<<")";

      m_sendToDds->rxData(msg);
    });


    uav->set_ReportCBitStatus_Callback([this, droneEqId](const BuiltInTest::ReportCBitStatus::Message &data)
    {
      Q_UNUSED(droneEqId)
      Q_UNUSED(data)
      Q_UNUSED(this)
    });


    uav->set_ReportDeviceStatusType_Callback([this, droneEqId](const Device::ReportDeviceStatus::Message &data)
    {
      TrainingAreaBase::TetraIcd::UAVStatus msg;
      msg.header.equipmentNumber = droneEqId;
      //now we need to handle the stuff
      switch (data.Status)
      {
      case Device::OperationModeStateKind::Auto:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Auto;
        break;
      case Device::OperationModeStateKind::Calibrating:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Calibrating;
        break;
      case Device::OperationModeStateKind::Init:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Init;
        break;
      case Device::OperationModeStateKind::Maintenance:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Maintenance;
        break;
      case Device::OperationModeStateKind::Manual:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Manual;
        setMissionActionFulfilled(droneEqId);
        break;
      case Device::OperationModeStateKind::Normal:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Normal;
        break;
      case Device::OperationModeStateKind::Off:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Off;
        break;
      case Device::OperationModeStateKind::On:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::On;
        break;
      case Device::OperationModeStateKind::Undefined:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Undefined;
        break;
      case Device::OperationModeStateKind::Standby:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Standby;
        break;
      case Device::OperationModeStateKind::Shutdown:
        msg.uavState = TrainingAreaBase::TetraIcd::UAVState::Shutdown;
        break;
      }

      m_sendToDds->rxData(msg);
      DEBUG_LOG(m_logger) << "RadioLinkConnectionState! sending RadioLinkConnectionState into TAB"; // << " ("<<(quintptr)this<<")";
    });


    uav->set_ReportRadioLinkConnectionState_Callback([this, droneEqId](const TeleOperation::RadioLinkConnectionState::Message &data)
    {
      DEBUG_LOG(m_logger) << "COULD! send  RadioLinkConnectionState! sending RadioLinkConnectionState into TAB << " << droneEqId  << " " << data.status; // << " ("<<(quintptr)this<<")";
    });


    uav->set_ReportMissionStatus_Callback([this, droneEqId](const MissionCtrl::ReportMissionStatus::Message &data)
    {
      DEBUG_LOG(m_logger) << "ReportMissionStatus! sending ReportMissionStatus into TAB << " << droneEqId  << " - currentTask = " << (int)data.currentTask << ", currentMission = " << data.currentMissionName;

      TrainingAreaBase::TetraIcd::UAVMissionStatus msg;
      msg.header.equipmentNumber = droneEqId;
      msg.missionEnded = QString::fromStdString(data.missionEnded);
      msg.currentMissionName = QString::fromStdString(data.currentMissionName);
      // NOTE: currentlyAllowedTasks ignored here

      switch (data.currentTask)
      {
      case MissionCtrl::Task::Disarm: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Disarm; break;
      case MissionCtrl::Task::Arm: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Arm; break;
      case MissionCtrl::Task::Takeoff: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Takeoff; break;
      case MissionCtrl::Task::Connect: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Connect; break;
      case MissionCtrl::Task::Loiter: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Loiter; break;
      case MissionCtrl::Task::Return: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Return;
        DEBUG_LOG(m_logger) << "Return";
        setMissionActionFulfilled(droneEqId);
        break;
      case MissionCtrl::Task::Observe: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Observe; break;
        // NOTE: all other cases are unused and will be returned as undefined
      default: msg.currentAction = TrainingAreaBase::TetraIcd::UAVActionTask::Undefined; break;
      }

      m_sendToDds->rxData(msg);
    });

    uav->startIA(config.m_ip.toStdString(), config.m_port, config.m_useTcp);

    DEBUG_LOG(m_logger) << "DONE STARTING UAV!";
  }
  catch (const std::exception &ex)
  {
    //try again later??
    WARNING_LOG(m_logger) << "Failed to start UAV: " << ex.what();
  }
}


void UAVInterfaceAdapterManager::stop(const ParticipantManagement::UAS &uas)
{
  DEBUG_LOG(m_logger) << "############################### STOP !! " << uas.id();
  auto it = m_uavs.find(uas.id());
  if (it == m_uavs.cend())
  {
    DEBUG_LOG(m_logger) << "uav not found - ignoring :/";
    return;
  }
  it->second->deleteLater();
  m_uavs.erase(it);
}


bool UAVInterfaceAdapterManager::kill(int participantId)
{
  const auto uas = m_dtcPpm->getUasByParticipantId(participantId, m_exerciseId);

  if (uas.id() < 1)
  {
    DEBUG_LOG(m_logger) << "uas by participant id " << participantId << " not found - stop";
    return false;
  }

  auto i = m_uavs.find(uas.id());

  if (i == m_uavs.cend())
  {
    DEBUG_LOG(m_logger) << "uav by participant id " << participantId << " not found - stop";
    return false;
  }

  auto &uav = *i;

  uav.second->commander().returnToHome();
  return true;
}


bool UAVInterfaceAdapterManager::waypoint(int participantId, TrainingAreaBase::TetraIcd::AbsolutePosition absPos, const QString &missionName)
{
  const auto uas = m_dtcPpm->getUasByParticipantId(participantId, m_exerciseId);

  if (uas.id() < 1)
  {
    DEBUG_LOG(m_logger) << "uas by participant id " << participantId << " not found - stop";
    return false;
  }

  auto i = m_uavs.find(uas.id());

  if (i == m_uavs.cend())
  {
    DEBUG_LOG(m_logger) << "uav by participant id " << participantId << " not found - stop";

    for (const auto &uav : m_uavs)
    {
      DEBUG_LOG(m_logger) << "pid: " << uav.first;
    }

    return false;
  }

  if (i->second == nullptr)
  {
    WARNING_LOG(m_logger) << "UAVInterfaceAdapterManager::waypoint: NULLPTR for "<<participantId;
    return false;
  }

  auto &uav = *i;

  try
  {
    // I dont think z will fit for alt
    uav.second->commander().sendMission(absPos.lat, absPos.lon, absPos.z, missionName);
    return true;
  }
  catch (const std::exception& e)
  {
    WARNING_LOG(m_logger) << "UAVInterfaceAdapterManager::waypoint: " << e.what();
    return false;
  }
}


void UAVInterfaceAdapterManager::setMissionActionFulfilled(int equipmentNumber)
{
  ///- send setFulfillment request to svcParticipantManagement

  DEBUG_LOG(m_logger) << "setMissionActionFulfilled equipmentId=" << equipmentNumber;

  const int missionId = m_dtcPpm->uasMissionIdByEquipmentNumber(equipmentNumber); // Todo get from equipmentId

  ParticipantManagement::SetMissionActionFulfilledRequest abortReq;
  abortReq.missionId(missionId);
  abortReq.itemNumber(-1);
  abortReq.actionNumber(-1);

  try
  {
    m_ftcPpm->setMissionActionFulfilledRequest(abortReq);
  }
  catch (const std::exception &e)
  {
    WARNING_LOG(m_logger) << " -- unable to abort mission with planId:"<<abortReq.missionId() << ": " << e.what();
  } 

  return;
}

} // namespace UAS