#include "behaviortree_cpp/blackboard.h"

#include "behaviortree_cpp/json_export.h"

#include <tuple>

#include <unordered_set>

namespace BT

{

namespace

{

bool IsPrivateKey(StringView str)

{

  return str.size() >= 1 && str.data()[0] == '_';

}

}  // namespace

void Blackboard::enableAutoRemapping(bool remapping)

{

  autoremapping_ = remapping;

}

AnyPtrLocked Blackboard::getAnyLocked(const std::string& key)

{

  if(auto entry = getEntry(key))

  {

    return AnyPtrLocked(&entry->value, &entry->entry_mutex);

  }

  return {};

}

AnyPtrLocked Blackboard::getAnyLocked(const std::string& key) const

{

  if(auto entry = getEntry(key))

  {

    return AnyPtrLocked(&entry->value, const_cast<std::mutex*>(&entry->entry_mutex));

  }

  return {};

}

const Any* Blackboard::getAny(const std::string& key) const

{

  return getAnyLocked(key).get();

}

Any* Blackboard::getAny(const std::string& key)

{

  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast)

  return const_cast<Any*>(getAnyLocked(key).get());

}

const std::shared_ptr<Blackboard::Entry>

Blackboard::getEntry(const std::string& key) const

{

  // special syntax: "@" will always refer to the root BB

  if(StartWith(key, '@'))

  {

    return rootBlackboard()->getEntry(key.substr(1, key.size() - 1));

  }

  {

    const std::unique_lock<std::mutex> storage_lock(storage_mutex_);

    auto it = storage_.find(key);

    if(it != storage_.end())

    {

      return it->second;

    }

  }

  // not found. Try autoremapping

  if(auto parent = parent_bb_.lock())

  {

    auto remap_it = internal_to_external_.find(key);

    if(remap_it != internal_to_external_.cend())

    {

      auto const& new_key = remap_it->second;

      return parent->getEntry(new_key);

    }

    if(autoremapping_ && !IsPrivateKey(key))

    {

      return parent->getEntry(key);

    }

  }

  return {};

}

std::shared_ptr<Blackboard::Entry> Blackboard::getEntry(const std::string& key)

{

  return static_cast<const Blackboard&>(*this).getEntry(key);

}

const TypeInfo* Blackboard::entryInfo(const std::string& key)

{

  auto entry = getEntry(key);

  return (!entry) ? nullptr : &(entry->info);

}

void Blackboard::addSubtreeRemapping(StringView internal, StringView external)

{

  internal_to_external_.insert(

      { static_cast<std::string>(internal), static_cast<std::string>(external) });

}

void Blackboard::debugMessage() const

{

  for(const auto& [key, entry] : storage_)

  {

    auto port_type = entry->info.type();

    if(port_type == typeid(void))

    {

      port_type = entry->value.type();

    }

    std::cout << key << " (" << BT::demangle(port_type) << ")" << std::endl;

  }

  for(const auto& [from, to] : internal_to_external_)

  {

    std::cout << "[" << from << "] remapped to port of parent tree [" << to << "]"

              << std::endl;

  }

}

std::vector<StringView> Blackboard::getKeys() const

{

  if(storage_.empty())

  {

    return {};

  }

  std::vector<StringView> out;

  out.reserve(storage_.size());

  for(const auto& entry_it : storage_)

  {

    out.push_back(entry_it.first);

  }

  return out;

}

void Blackboard::clear()

{

  const std::unique_lock<std::mutex> storage_lock(storage_mutex_);

  storage_.clear();

}

std::recursive_mutex& Blackboard::entryMutex() const

{

  return entry_mutex_;

}

void Blackboard::createEntry(const std::string& key, const TypeInfo& info)

{

  if(StartWith(key, '@'))

  {

    if(key.find('@', 1) != std::string::npos)

    {

      throw LogicError("Character '@' used multiple times in the key");

    }

    rootBlackboard()->createEntryImpl(key.substr(1, key.size() - 1), info);

  }

  else

  {

    createEntryImpl(key, info);

  }

}

void Blackboard::cloneInto(Blackboard& dst) const

{

  // Lock both mutexes without risking lock-order inversion.

  std::unique_lock<std::mutex> lk1(storage_mutex_, std::defer_lock);

  std::unique_lock<std::mutex> lk2(dst.storage_mutex_, std::defer_lock);

  std::lock(lk1, lk2);

  // keys that are not updated must be removed.

  std::unordered_set<std::string> keys_to_remove;

  auto& dst_storage = dst.storage_;

  for(const auto& [key, entry] : dst_storage)

  {

    std::ignore = entry;  // unused in this loop

    keys_to_remove.insert(key);

  }

  // update or create entries in dst_storage

  for(const auto& [src_key, src_entry] : storage_)

  {

    keys_to_remove.erase(src_key);

    auto it = dst_storage.find(src_key);

    if(it != dst_storage.end())

    {

      // overwrite

      auto& dst_entry = it->second;

      dst_entry->string_converter = src_entry->string_converter;

      dst_entry->value = src_entry->value;

      dst_entry->info = src_entry->info;

      dst_entry->sequence_id++;

      dst_entry->stamp = std::chrono::steady_clock::now().time_since_epoch();

    }

    else

    {

      // create new

      auto new_entry = std::make_shared<Entry>(src_entry->info);

      new_entry->value = src_entry->value;

      new_entry->string_converter = src_entry->string_converter;

      dst_storage.insert({ src_key, new_entry });

    }

  }

  for(const auto& key : keys_to_remove)

  {

    dst_storage.erase(key);

  }

}

Blackboard::Ptr Blackboard::parent()

{

  if(auto parent = parent_bb_.lock())

  {

    return parent;

  }

  return {};

}

std::shared_ptr<Blackboard::Entry> Blackboard::createEntryImpl(const std::string& key,

                                                               const TypeInfo& info)

{

  const std::unique_lock<std::mutex> storage_lock(storage_mutex_);

  // This function might be called recursively, when we do remapping, because we move

  // to the top scope to find already existing  entries

  // search if exists already

  auto storage_it = storage_.find(key);

  if(storage_it != storage_.end())

  {

    const auto& prev_info = storage_it->second->info;

    if(prev_info.type() != info.type() && prev_info.isStronglyTyped() &&

       info.isStronglyTyped())

    {

      auto msg = StrCat("Blackboard entry [", key,

                        "]: once declared, the type of a port"

                        " shall not change. Previously declared type [",

                        BT::demangle(prev_info.type()), "], current type [",

                        BT::demangle(info.type()), "]");

      throw LogicError(msg);

    }

    return storage_it->second;

  }

  // manual remapping first

  auto remapping_it = internal_to_external_.find(key);

  if(remapping_it != internal_to_external_.end())

  {

    const auto& remapped_key = remapping_it->second;

    if(auto parent = parent_bb_.lock())

    {

      return parent->createEntryImpl(remapped_key, info);

    }

    throw RuntimeError("Missing parent blackboard");

  }

  // autoremapping second (excluding private keys)

  if(autoremapping_ && !IsPrivateKey(key))

  {

    if(auto parent = parent_bb_.lock())

    {

      return parent->createEntryImpl(key, info);

    }

    throw RuntimeError("Missing parent blackboard");

  }

  // not remapped, not found. Create locally.

  auto entry = std::make_shared<Entry>(info);

  // even if empty, let's assign to it a default type

  entry->value = Any(info.type());

  storage_.insert({ key, entry });

  return entry;

}

nlohmann::json ExportBlackboardToJSON(const Blackboard& blackboard)

{

  nlohmann::json dest;

  for(auto entry_name : blackboard.getKeys())

  {

    const std::string name(entry_name);

    if(auto any_ref = blackboard.getAnyLocked(name))

    {

      if(auto any_ptr = any_ref.get())

      {

        JsonExporter::get().toJson(*any_ptr, dest[name]);

      }

    }

  }

  return dest;

}

void ImportBlackboardFromJSON(const nlohmann::json& json, Blackboard& blackboard)

{

  for(auto it = json.begin(); it != json.end(); ++it)

  {

    if(auto res = JsonExporter::get().fromJson(it.value()))

    {

      auto entry = blackboard.getEntry(it.key());

      if(!entry)

      {

        blackboard.createEntry(it.key(), res->second);

        entry = blackboard.getEntry(it.key());

      }

      entry->value = res->first;

    }

  }

}

Blackboard* BT::Blackboard::rootBlackboard()

{

  auto bb = static_cast<const Blackboard&>(*this).rootBlackboard();

  // NOLINTNEXTLINE(cppcoreguidelines-pro-type-const-cast)

  return const_cast<Blackboard*>(bb);

}

const Blackboard* BT::Blackboard::rootBlackboard() const

{

  const Blackboard* bb = this;

  Blackboard::Ptr prev = parent_bb_.lock();

  while(prev)

  {

    bb = prev.get();

    prev = bb->parent_bb_.lock();

  }

  return bb;

}

}  // namespace BT