#include "wait_set_handler.h"
#include "generic_logger.h"
#include "generic_base.h"

namespace DDSGeneric
{

WaitSetHandler::WaitSetHandler()
  : m_logger(DDS::getLogger("WaitSetHandler", DDS::DataType::FUNCTION_TOPIC_SERVER))
{
  m_waitSet.attach_condition(m_abortCondition);
  m_task = std::async(std::launch::async, [this]() { this->waitSetTask();});
}

WaitSetHandler::~WaitSetHandler()
{
  m_abortCondition.trigger_value(true);
  m_task.wait();
}

bool WaitSetHandler::attachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
{
  DEBUG_LOG(m_logger) << "AttachWaitSet with callback " << cb;

  WaitSetData wsd(cond, cb);

  std::lock_guard<std::mutex> l(m_accessMutex);
  
  auto it = m_waitSetData.find(wsd.condition->native_condition());
  if (it != m_waitSetData.end())
  {
    WARNING_LOG(m_logger) << "Waitset " << cb << " already registered";
    return false;
  }

  m_waitSet.attach_condition(wsd.condition);
  wsd.enabled = true;
  wsd.needWaitSetAttach = false;

  m_waitSetData.insert(std::make_pair(wsd.condition->native_condition(), wsd));
  return true;
}

bool WaitSetHandler::detachWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
{
  DEBUG_LOG(m_logger) << "DetachWaitSet with callback " << cb;

  WaitSetData wsd(cond, cb);

  std::lock_guard<std::mutex> l(m_accessMutex);
  
  auto it = m_waitSetData.find(wsd.condition->native_condition());
  if (it == m_waitSetData.end())
  {
    WARNING_LOG(m_logger) << "Waitset for " << cb << " not registered";
    return false;
  }

  m_waitSet.detach_condition(wsd.condition);
  m_waitSetData.erase(it);
  return true;
}

bool WaitSetHandler::enableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
{
  DEBUG_LOG(m_logger) << "EnableWaitSet with callback " << cb;

  WaitSetData wsd(cond, cb);

  std::lock_guard<std::mutex> l(m_accessMutex);
  auto it = m_waitSetData.find(wsd.condition->native_condition());
  if (it == m_waitSetData.end())
  {
    ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot enable waitSet for " << cb;
    return false;
  }

  if (it->second.needWaitSetAttach)
    m_waitSet.attach_condition(wsd.condition);
  it->second.enabled = true;
  it->second.needWaitSetAttach = false;
  return true;
}

bool WaitSetHandler::disableWaitSet(const dds::core::cond::Condition &cond, CallbackInterface *cb)
{
  DEBUG_LOG(m_logger) << "DisableWaitSet with callback " << cb;

  WaitSetData wsd(cond, cb);

  std::lock_guard<std::mutex> l(m_accessMutex);
  auto it = m_waitSetData.find(wsd.condition->native_condition());
  if (it == m_waitSetData.end())
  {
    ERROR_LOG(m_logger) << "Could not find WaitSetData - cannot disable waitSet for  " << cb;
    return false;
  }

  m_waitSet.detach_condition(wsd.condition);
  it->second.enabled = false;
  it->second.needWaitSetAttach = true;
  return true;
}

int WaitSetHandler::attachedConditionCount() const
{
  return m_waitSet.conditions().size();
}


void WaitSetHandler::waitSetTask()
{
  while (true)
  {
    dds::core::cond::WaitSet::ConditionSeq activeConditionsSeq = m_waitSet.wait();
    for (const dds::core::cond::Condition &cond : activeConditionsSeq)
    {
      if (cond == m_abortCondition)
      {
        m_waitSet->detach_all();
        return;
      }

      std::lock_guard<std::mutex> l(m_accessMutex);
      const DDS_Condition *ddsCond = cond->native_condition();
      auto it = m_waitSetData.lower_bound(ddsCond);
      while (it != m_waitSetData.end())
      {
        WaitSetData &wsd = it->second;
        if (wsd.enabled)
        {
          DEBUG_LOG(m_logger) << "Condition triggered for " << wsd.callback;
          wsd.callback->dataAvailable();
        }
        ++it;
        if (it == m_waitSetData.end() || it->first != ddsCond)
          break;
      }
    }
  }
}

}