text:
#include "nexus/scheduler/scheduler.h"

#include <glog/logging.h>

#include <boost/filesystem.hpp>
#include <cmath>
#include <unordered_set>

#include "nexus/common/config.h"
#include "nexus/common/model_db.h"

namespace fs = boost::filesystem;

DEFINE_bool(epoch_schedule, true, "Enable epoch scheduling");
DEFINE_bool(prefix_batch, true, "Enable prefix batching");
DEFINE_int32(beacon, 1, "Beacon interval in seconds");
DEFINE_int32(epoch, 30, "Epoch scheduling interval in seconds");
DEFINE_int32(min_epoch, 10,
             "Minimum time interval in seconds to invoke "
             "epoch schedule");
DEFINE_int32(avg_interval, 10, "Moving average interval for backend rate");

namespace nexus {
namespace scheduler {

INSTANTIATE_RPC_CALL(AsyncService, Register, RegisterRequest, RegisterReply);
INSTANTIATE_RPC_CALL(AsyncService, Unregister, UnregisterRequest, RpcReply);
INSTANTIATE_RPC_CALL(AsyncService, LoadModel, LoadModelRequest, LoadModelReply);
INSTANTIATE_RPC_CALL(AsyncService, ReportWorkload, WorkloadStatsProto,
                     RpcReply);
INSTANTIATE_RPC_CALL(AsyncService, KeepAlive, KeepAliveRequest, RpcReply);
INSTANTIATE_RPC_CALL(AsyncService, ComplexQuerySetup, ComplexQuerySetupRequest,
                     RpcReply);
INSTANTIATE_RPC_CALL(AsyncService, ComplexQueryAddEdge,
                     ComplexQueryAddEdgeRequest, RpcReply);

Scheduler::Scheduler(std::string port, size_t nthreads)
    : AsyncRpcServiceBase(port, nthreads),
      beacon_interval_sec_(FLAGS_beacon),
      epoch_interval_sec_(FLAGS_epoch),
      enable_epoch_schedule_(FLAGS_epoch_schedule),
      enable_prefix_batch_(FLAGS_prefix_batch) {
  history_len_ = (FLAGS_avg_interval * 3 + beacon_interval_sec_ - 1) /
                 beacon_interval_sec_;
  if (!enable_epoch_schedule_) {
    LOG(INFO) << "Epoch scheduling is off";
  }
  if (!enable_prefix_batch_) {
    LOG(INFO) << "Prefix batching is off";
  }
}

void Scheduler::LoadWorkloadFile(const std::string& workload_file) {
  LOG(INFO) << "Load workload file from " << workload_file;
  YAML::Node config = YAML::LoadFile(workload_file);
  // Load static workload configuration
  for (uint i = 0; i < config.size(); ++i) {
    const YAML::Node& backend_workload = config[i];
    LOG(INFO) << "Backend " << i << ":";
    std::vector<YAML::Node> models;
    for (uint j = 0; j < backend_workload.size(); ++j) {
      LOG(INFO) << "- " << backend_workload[j];
      models.push_back(backend_workload[j]);
    }
    static_workloads_.push_back(models);
  }
}

void Scheduler::Run() {
  // Start RPC service first
  Start();
  // main scheduler login
  std::this_thread::sleep_for(std::chrono::seconds(beacon_interval_sec_));
  auto last_epoch_schedule = std::chrono::system_clock::now();
  while (running_) {
    auto now = std::chrono::system_clock::now();
    bool trigger = BeaconCheck();
    if (enable_epoch_schedule_) {
      if (trigger) {
        auto elapse = std::chrono::duration_cast<std::chrono::milliseconds>(
                          now - last_epoch_schedule)
                          .count();
        if (elapse >= FLAGS_min_epoch * 1000) {
          EpochSchedule();
          last_epoch_schedule = std::chrono::system_clock::now();
        }
      } else {
        auto elapse = std::chrono::duration_cast<std::chrono::milliseconds>(
                          now - last_epoch_schedule)
                          .count();
        if (elapse >= epoch_interval_sec_ * 1000) {
          EpochSchedule();
          last_epoch_schedule = std::chrono::system_clock::now();
        }
      }
    }
    std::this_thread::sleep_for(std::chrono::seconds(beacon_interval_sec_));
  }
}

void Scheduler::Register(const grpc::ServerContext& ctx,
                         const RegisterRequest& request, RegisterReply* reply) {
  std::vector<std::string> tokens;
  SplitString(ctx.peer(), ':', &tokens);
  std::string ip = tokens[1];
  LOG(INFO) << "Register server: " << request.DebugString();
  if (request.node_type() == BACKEND_NODE) {
    auto backend = std::make_shared<BackendDelegate>(
        request.node_id(), ip, request.server_port(), request.rpc_port(),
        request.gpu_device_name(), request.gpu_uuid(),
        request.gpu_available_memory(), beacon_interval_sec_);
    RegisterBackend(std::move(backend), reply);
  } else {  // FRONTEND_NODE or DISPATCHER_NODE
    bool is_dispatcher = request.node_type() == NodeType::DISPATCHER_NODE;
    auto frontend = std::make_shared<FrontendDelegate>(
        request.node_id(), ip, request.server_port(), request.rpc_port(),
        beacon_interval_sec_, is_dispatcher);
    RegisterFrontend(std::move(frontend), reply);
  }
}

void Scheduler::Unregister(const grpc::ServerContext& ctx,
                           const UnregisterRequest& request, RpcReply* reply) {
  LOG(INFO) << "Unregister " << NodeType_Name(request.node_type()) << " "
            << request.node_id();
  if (request.node_type() == BACKEND_NODE) {
    UnregisterBackend(request.node_id());
  } else {  // FRONTEND_NODE
    bool is_dispatcher = request.node_type() == NodeType::DISPATCHER_NODE;
    UnregisterFrontend(request.node_id(), is_dispatcher);
  }
  reply->set_status(CTRL_OK);
}

void Scheduler::LoadModel(const grpc::ServerContext& ctx,
                          const LoadModelRequest& request,
                          LoadModelReply* reply) {
  LoadModelInternal(ctx, request, reply);

  // Update the dispatcher
  if (reply->status() == CTRL_OK) {
    ModelRouteUpdates dispatcher_update;
    *dispatcher_update.add_model_route() = reply->model_route();
    for (const auto& iter : dispatchers_) {
      iter.second->UpdateModelRoutesRpc(dispatcher_update);
    }
  }
}

void Scheduler::LoadModelInternal(const grpc::ServerContext& ctx,
                                  const LoadModelRequest& request,
                                  LoadModelReply* reply) {
  ModelSession model_sess(request.model_session());
  {
    auto info = ModelDatabase::Singleton().GetModelInfo(
        ModelSessionToModelID(model_sess));
    if (info == nullptr) {
      reply->set_status(MODEL_NOT_FOUND);
      return;
    }
    if ((*info)["resizable"] && (*info)["resizable"].as<bool>()) {
      if (model_sess.image_height() == 0) {
        // Set default image size for resizable CNN
        model_sess.set_image_height((*info)["image_height"].as<uint32_t>());
        model_sess.set_image_width((*info)["image_width"].as<uint32_t>());
      }
    }
  }
  std::string model_sess_id = ModelSessionToString(model_sess);
  double workload = request.estimate_workload();

  std::lock_guard<std::mutex> lock(mutex_);
  auto frontend = GetFrontend(request.node_id());
  if (frontend == nullptr) {
    reply->set_status(CTRL_SERVER_NOT_REGISTERED);
    return;
  }
  auto session_iter = session_table_.find(model_sess_id);
  if (session_iter != session_table_.end()) {
    // TODO: For now, if model session is already loaded, don't allocate
    // new backends, just rely on epoch scheduling
    reply->set_status(CTRL_OK);
    GetModelRoute(model_sess_id, reply->mutable_model_route());
    frontend->SubscribeModel(model_sess_id);
    session_iter->second->SubscribeModelSession(frontend->node_id(),
                                                model_sess_id);
    return;
  }

  // Check prefix batching first
  if (enable_prefix_batch_) {
    // Prefix batching is enabled
    std::string model_id = ModelSessionToModelID(model_sess);
    auto share_prefixes =
        ModelDatabase::Singleton().GetPrefixShareModels(model_id);
    if (!share_prefixes.empty()) {
      std::shared_ptr<SessionInfo> share_session_info = nullptr;
      ModelSession share_model_sess;
      share_model_sess.set_image_height(model_sess.image_height());
      share_model_sess.set_image_width(model_sess.image_width());
      // TODO: currently only support prefix batching with same latency sla
      share_model_sess.set_latency_sla(model_sess.latency_sla());
      // Find if there are model sessions that can share prefix with
      for (auto share_model_id : share_prefixes) {
        ParseModelID(share_model_id, &share_model_sess);
        std::string share_model_sess_id =
            ModelSessionToString(share_model_sess);
        auto iter = session_table_.find(share_model_sess_id);
        if (iter != session_table_.end()) {
          share_session_info = iter->second;
          break;
        }
      }
      if (share_session_info != nullptr) {
        // Find shared model session
        LOG(INFO) << "Model session " << model_sess_id
                  << " shares prefix "
                     "with session "
                  << ModelSessionToString(share_model_sess);
        for (auto iter : share_session_info->backend_weights) {
          auto backend = GetBackend(iter.first);
          backend->LoadPrefixModel(model_sess, share_model_sess);
          backend->UpdateModelTableRpc();
        }
        share_session_info->model_sessions.push_back(model_sess);
        session_table_.emplace(model_sess_id, share_session_info);
        frontend->SubscribeModel(model_sess_id);
        share_session_info->SubscribeModelSession(frontend->node_id(),
                                                  model_sess_id);
        // Fill route table in the reply
        reply->set_status(CTRL_OK);
        GetModelRoute(model_sess_id, reply->mutable_model_route());
        return;
      }
    }
  }

  // Check TFShare models
  if (enable_prefix_batch_) do {
      // FIXME this code block is almost the same as the previous one. need
      // refactor.
      if (model_sess.framework() != "tf_share") break;
      auto tf_share_info =
          ModelDatabase::Singleton().GetTFShareInfo(model_sess.model_name());
      if (!tf_share_info) break;

      ModelSession share_model_sess;
      share_model_sess.set_framework("tf_share");
      share_model_sess.set_version(model_sess.version());
      share_model_sess.set_latency_sla(model_sess.latency_sla());

      // Find if there are model sessions that can share prefix with
      std::shared_ptr<SessionInfo> share_session_info = nullptr;
      for (const auto& suffix : tf_share_info->suffix_models) {
        share_model_sess.set_model_name(suffix.first);
        auto suffix_model_id = ModelSessionToString(share_model_sess);
        auto iter = session_table_.find(suffix_model_id);
        if (iter != session_table_.end()) {
          share_session_info = iter->second;
          break;
        }
      }
      if (!share_session_info) break;

      // Find shared model session
      LOG(INFO) << "Model session " << model_sess_id
                << " shares prefix with session "
                << ModelSessionToString(share_model_sess);
      for (auto iter : share_session_info->backend_weights) {
        auto backend = GetBackend(iter.first);
        backend->LoadPrefixModel(model_sess, share_model_sess);
        backend->UpdateModelTableRpc();
      }
      share_session_info->model_sessions.push_back(model_sess);
      session_table_.emplace(model_sess_id, share_session_info);
      frontend->SubscribeModel(model_sess_id);
      share_session_info->SubscribeModelSession(frontend->node_id(),
                                                model_sess_id);
      // Fill route table in the reply
      reply->set_status(CTRL_OK);
      GetModelRoute(model_sess_id, reply->mutable_model_route());
      return;
    } while (false);

  // Find best-fit backends to serve the workload
  std::vector<std::pair<BackendDelegatePtr, InstanceInfo> > assign_backends;
  std::unordered_set<uint32_t> used;
  if (workload == 0) {
    BackendDelegatePtr backend;
    InstanceInfo inst_info;
    FindBestBackend(model_sess, workload, used, &backend, &inst_info);
    if (backend == nullptr) {
      reply->set_status(NOT_ENOUGH_BACKENDS);
      return;
    }
    assign_backends.emplace_back(backend, inst_info);
  } else {
    while (workload > 1e-3) {
      BackendDelegatePtr backend;
      InstanceInfo inst_info;
      FindBestBackend(model_sess, workload, used, &backend, &inst_info);
      if (backend == nullptr) {
        reply->set_status(NOT_ENOUGH_BACKENDS);
        return;
      }
      assign_backends.emplace_back(backend, inst_info);
      used.insert(backend->node_id());
      workload -= inst_info.throughput;
    }
  }

  // Load models
  auto session_info = std::make_shared<SessionInfo>();
  for (auto iter : assign_backends) {
    auto backend = iter.first;
    auto const& inst_info = iter.second;
    backend->LoadModel(inst_info);
    backend->UpdateModelTableRpc();
    session_info->backend_weights.emplace(backend->node_id(),
                                          inst_info.GetWeight());
  }
  session_info->model_sessions.push_back(model_sess);
  session_table_.emplace(model_sess_id, session_info);
  frontend->SubscribeModel(model_sess_id);
  session_info->SubscribeModelSession(frontend->node_id(), model_sess_id);

  // Fill route table in the reply
  reply->set_status(CTRL_OK);
  GetModelRoute(model_sess_id, reply->mutable_model_route());
}

void Scheduler::ReportWorkload(const grpc::ServerContext& ctx,
                               const WorkloadStatsProto& request,
                               RpcReply* reply) {
  std::lock_guard<std::mutex> lock(mutex_);
  auto frontend = GetFrontend(request.node_id());
  if (frontend == nullptr) {
    reply->set_status(CTRL_SERVER_NOT_REGISTERED);
    return;
  }
  frontend->Tick();
  for (auto const& model_stats : request.model_stats()) {
    auto session_info = session_table_.at(model_stats.model_session_id());
    session_info->UpdateWorkload(request.node_id(), model_stats);
  }
  reply->set_status(CTRL_OK);
}

void Scheduler::KeepAlive(const grpc::ServerContext& ctx,
                          const KeepAliveRequest& request, RpcReply* reply) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (request.node_type() == FRONTEND_NODE) {
    auto frontend = GetFrontend(request.node_id());
    if (frontend == nullptr) {
      reply->set_status(CTRL_SERVER_NOT_REGISTERED);
      return;
    }
    frontend->Tick();
  } else {
    auto backend = GetBackend(request.node_id());
    if (backend == nullptr) {
      reply->set_status(CTRL_SERVER_NOT_REGISTERED);
      return;
    }
    backend->Tick();
  }
  reply->set_status(CTRL_OK);
}

void Scheduler::ComplexQuerySetup(const grpc::ServerContext& ctx,
                                  const ComplexQuerySetupRequest& request,
                                  RpcReply* reply) {
  std::lock_guard<std::mutex> lock(mutex_);
  // TODO: return error instead of crash
  CHECK(request.cq_id().empty()) << "empty cq_id";
  CHECK(complex_queries_.count(request.cq_id()) == 0)
      << "Complex Query " << request.cq_id() << " has already been set up.";
  ComplexQuery cq(request.cq_id(), request.slo_us(), request.step_us());
  complex_queries_.emplace(request.cq_id(), std::move(cq));
  reply->set_status(CTRL_OK);
}

void Scheduler::ComplexQueryAddEdge(const grpc::ServerContext& ctx,
                                    const ComplexQueryAddEdgeRequest& request,
                                    RpcReply* reply) {
  std::lock_guard<std::mutex> lock(mutex_);
  // TODO: return error instead of crash
  auto src_model_sess_id = ModelSessionToString(request.source());
  auto dst_model_sess_id = ModelSessionToString(request.target());
  auto iter_cq = complex_queries_.find(request.cq_id());
  auto iter_src = session_table_.find(src_model_sess_id);
  auto iter_dst = session_table_.find(dst_model_sess_id);
  CHECK(iter_cq != complex_queries_.end())
      << "Cannot find Complex Query " << request.cq_id();
  CHECK(iter_src != session_table_.end())
      << "Cannot find Model Session " << src_model_sess_id;
  CHECK(iter_dst != session_table_.end())
      << "Cannot find Model Session " << dst_model_sess_id;
  CHECK(iter_src->second->complex_query_id.empty() ||
        iter_src->second->complex_query_id == request.cq_id())
      << "Model Session " << src_model_sess_id
      << " has been linked to another Complex Query "
      << iter_src->second->complex_query_id;
  CHECK(iter_dst->second->complex_query_id.empty() ||
        iter_dst->second->complex_query_id == request.cq_id())
      << "Model Session " << dst_model_sess_id
      << " has been linked to another Complex Query "
      << iter_dst->second->complex_query_id;

  iter_src->second->complex_query_id = request.cq_id();
  iter_dst->second->complex_query_id = request.cq_id();
}

void Scheduler::HandleRpcs() {
  using namespace std::placeholders;
  new Register_Call(&service_, cq_.get(),
                    std::bind(&Scheduler::Register, this, _1, _2, _3));
  new Unregister_Call(&service_, cq_.get(),
                      std::bind(&Scheduler::Unregister, this, _1, _2, _3));
  new LoadModel_Call(&service_, cq_.get(),
                     std::bind(&Scheduler::LoadModel, this, _1, _2, _3));
  new ReportWorkload_Call(
      &service_, cq_.get(),
      std::bind(&Scheduler::ReportWorkload, this, _1, _2, _3));
  new KeepAlive_Call(&service_, cq_.get(),
                     std::bind(&Scheduler::KeepAlive, this, _1, _2, _3));
  new ComplexQuerySetup_Call(
      &service_, cq_.get(),
      std::bind(&Scheduler::ComplexQuerySetup, this, _1, _2, _3));
  new ComplexQueryAddEdge_Call(
      &service_, cq_.get(),
      std::bind(&Scheduler::ComplexQueryAddEdge, this, _1, _2, _3));
  void* tag;
  bool ok;
  while (running_) {
    cq_->Next(&tag, &ok);
    if (ok) {
      static_cast<RpcCallBase*>(tag)->Proceed();
    }
  }
}

void Scheduler::RegisterFrontend(FrontendDelegatePtr frontend,
                                 RegisterReply* reply) {
  // lock protected
  std::lock_guard<std::mutex> lock(mutex_);
  auto& nodes = frontend->IsDispatcher() ? dispatchers_ : frontends_;
  if (nodes.find(frontend->node_id()) != nodes.end()) {
    reply->set_status(CTRL_FRONTEND_NODE_ID_CONFLICT);
    return;
  }
  // add the frontend client in the frontend map
  nodes[frontend->node_id()] = frontend;
  reply->set_status(CTRL_OK);
  reply->set_beacon_interval_sec(BEACON_INTERVAL_SEC);
}

void Scheduler::RegisterBackend(BackendDelegatePtr backend,
                                RegisterReply* reply) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (backends_.find(backend->node_id()) != backends_.end()) {
    reply->set_status(CTRL_BACKEND_NODE_ID_CONFLICT);
    return;
  }
  // Add the backend client in the backend map
  backends_[backend->node_id()] = backend;
  reply->set_status(CTRL_OK);
  reply->set_beacon_interval_sec(BEACON_INTERVAL_SEC);

  // Update workload to new backend
  AddBackend(backend);
}

void Scheduler::UnregisterFrontend(uint32_t node_id, bool is_dispatcher) {
  std::lock_guard<std::mutex> lock(mutex_);
  const char* node_type = nullptr;
  std::unordered_map<uint32_t, FrontendDelegatePtr>* nodes = nullptr;
  FrontendDelegatePtr node;
  if (is_dispatcher) {
    node_type = "dispatcher";
    nodes = &dispatchers_;
    node = GetDispatcher(node_id);
  } else {
    node_type = "frontend";
    nodes = &frontends_;
    node = GetFrontend(node_id);
  }

  if (!node) {
    LOG(ERROR) << "Cannot find " << node_type << " " << node_id;
    return;
  }
  nodes->erase(node->node_id());
  LOG(INFO) << "Remove " << node_type << " " << node_id;
  RemoveFrontend(node);
}

void Scheduler::UnregisterBackend(uint32_t node_id) {
  std::lock_guard<std::mutex> lock(mutex_);
  auto backend = GetBackend(node_id);
  if (backend == nullptr) {
    LOG(ERROR) << "Cannot find backend " << node_id;
    return;
  }
  backends_.erase(backend->node_id());
  LOG(INFO) << "Remove backend " << node_id;
  RemoveBackend(backend);
}

void Scheduler::AddBackend(BackendDelegatePtr backend) {
  std::unordered_set<SessionInfoPtr> changed_sessions;
  std::unordered_set<BackendDelegatePtr> changed_backends;

  // 1. Check if there is any static configured workload to assign
  int assign_load_id = -1;
  for (uint id = 0; id < static_workloads_.size(); ++id) {
    if (assigned_static_workloads_.find(id) ==
        assigned_static_workloads_.end()) {
      assign_load_id = id;
      assigned_static_workloads_[id] = backend->node_id();
      break;
    }
  }
  if (assign_load_id >= 0) {
    // Assign static workload
    LOG(INFO) << "Assign workload " << assign_load_id << " to backend "
              << backend->node_id();
    auto workload = static_workloads_[assign_load_id];
    for (auto session_info : workload) {
      backend->LoadModel(session_info);
    }
    backend->set_workload_id(assign_load_id);
    changed_backends.insert(backend);

    // Update session info
    for (auto const& model_sess_id : backend->GetModelSessions()) {
      if (session_table_.find(model_sess_id) == session_table_.end()) {
        auto session_info = std::make_shared<SessionInfo>();
        session_info->has_static_workload = true;
        ModelSession model_sess;
        ParseModelSession(model_sess_id, &model_sess);
        session_info->model_sessions.push_back(model_sess);
        session_table_.emplace(model_sess_id, session_info);
      }
      auto session_info = session_table_.at(model_sess_id);
      session_info->backend_weights.emplace(
          backend->node_id(), backend->GetModelWeight(model_sess_id));
      changed_sessions.insert(session_info);
      for (auto backup_id : session_info->backup_backends) {
        auto backup_backend = GetBackend(backup_id);
        if (backup_backend != nullptr) {
          BackendInfo backup_info;
          backup_backend->GetInfo(&backup_info);
          backend->AddBackupForModel(model_sess_id, backup_info);
        }
      }
    }
    // Add backup model to session info
    BackendInfo backend_info;
    backend->GetInfo(&backend_info);
    for (auto& model_sess_id : backend->GetBackupModelSessions()) {
      LOG(INFO) << "backup model session: " << model_sess_id;
      if (session_table_.find(model_sess_id) == session_table_.end()) {
        auto session_info = std::make_shared<SessionInfo>();
        session_info->has_static_workload = true;
        ModelSession model_sess;
        ParseModelSession(model_sess_id, &model_sess);
        session_info->model_sessions.push_back(model_sess);
        session_table_.emplace(model_sess_id, session_info);
      }
      auto session_info = session_table_.at(model_sess_id);
      if (session_info->backup_backends.count(backend->node_id()) > 0) {
        continue;
      }
      session_info->backup_backends.insert(backend->node_id());
      for (auto iter : session_info->backend_weights) {
        auto b = GetBackend(iter.first);
        if (b == nullptr) {
          continue;
        }
        b->AddBackupForModel(model_sess_id, backend_info);
        changed_backends.insert(b);
      }
    }
  } else {
    // 2. Check if there are unassigned workloads
    AllocateUnassignedWorkloads(&changed_sessions, &changed_backends);
    for (auto session : changed_sessions) {
      LOG(INFO) << "Changed session: "
                << ModelSessionToString(session->model_sessions[0]);
    }
  }

  // 3. Update backend model table
  for (auto b : changed_backends) {
    b->UpdateModelTableRpc();
  }

  // 4. Update model info and route
  UpdateModelRoutes(changed_sessions);
}

void Scheduler::RemoveBackend(BackendDelegatePtr backend) {
  if (backend->IsIdle()) {
    return;
  }
  std::unordered_set<SessionInfoPtr> changed_sessions;
  std::unordered_set<BackendDelegatePtr> changed_backends;

  // 1. Remove backend from ModelInfo
  std::vector<std::string> model_sessions = backend->GetModelSessions();
  for (auto& model_sess_id : model_sessions) {
    if (session_table_.count(model_sess_id) == 0) {
      continue;
    }
    auto session_info = session_table_.at(model_sess_id);
    // Because shared prefix models could share the same session_info,
    // it's necessary to check whether session_info is already in the changed
    // list
    if (changed_sessions.count(session_info) == 0) {
      session_info->backend_weights.erase(backend->node_id());
      changed_sessions.insert(session_info);
    }
  }

  // 2. Try to re-assign backend's workload to another idle one
  BackendDelegatePtr assigned;
  for (auto iter : backends_) {
    if (iter.second->IsIdle() && iter.second->Assign(*backend)) {
      assigned = iter.second;
      break;
    }
  }
  if (assigned != nullptr) {
    for (auto& model_sess_id : model_sessions) {
      session_table_.at(model_sess_id)
          ->backend_weights.emplace(
              assigned->node_id(), assigned->GetModelThroughput(model_sess_id));
    }
    if (assigned->workload_id() >= 0) {
      assigned_static_workloads_.emplace(assigned->workload_id(),
                                         assigned->node_id());
      LOG(INFO) << "Reassign workload " << assigned->workload_id()
                << " to backend " << assigned->node_id();
    }
    changed_backends.insert(assigned);
    // Remove backup models to session info
    for (auto& model_sess_id : backend->GetBackupModelSessions()) {
      auto session_info = session_table_.at(model_sess_id);
      session_info->backup_backends.erase(backend->node_id());
      session_info->backup_backends.insert(assigned->node_id());
      BackendInfo info;
      assigned->GetInfo(&info);
      for (auto iter : session_info->backend_weights) {
        auto b = GetBackend(iter.first);
        if (b != nullptr) {
          b->RemoveBackupForModel(model_sess_id, backend->node_id());
          b->AddBackupForModel(model_sess_id, info);
          changed_backends.insert(b);
        }
      }
    }
  } else {  // assigned == nullptr
    // Remove backup models to session info
    for (auto& model_sess_id : backend->GetBackupModelSessions()) {
      auto session_info = session_table_.at(model_sess_id);
      if (session_info->backup_backends.erase(backend->node_id()) == 0) {
        continue;
      }
      for (auto iter : session_info->backend_weights) {
        auto b = GetBackend(iter.first);
        if (b != nullptr) {
          b->RemoveBackupForModel(model_sess_id, backend->node_id());
          changed_backends.insert(b);
        }
      }
    }
    if (backend->workload_id() >= 0) {
      assigned_static_workloads_.erase(backend->workload_id());
      LOG(INFO) << "Remove workload " << backend->workload_id();
    } else {
      // 3. If it's not static configured workload, try to allocate model
      // instances to other backends
      for (auto& model_sess_id : model_sessions) {
        double tp = backend->GetModelThroughput(model_sess_id);
        session_table_.at(model_sess_id)->unassigned_workload += tp;
      }
      AllocateUnassignedWorkloads(&changed_sessions, &changed_backends);
      // TODO: assign backup models to other backends
    }
  }

  // 4. Update backend model table
  for (auto b : changed_backends) {
    b->UpdateModelTableRpc();
  }

  // 5. Update changed routes;
  UpdateModelRoutes(changed_sessions);
}

void Scheduler::RemoveFrontend(FrontendDelegatePtr frontend) {
  // Update subscribed model sessions
  std::unordered_set<BackendDelegatePtr> update_backends;
  for (auto model_sess_id : frontend->subscribe_models()) {
    auto session_info = session_table_.at(model_sess_id);
    bool remove = session_info->UnsubscribleModelSession(frontend->node_id(),
                                                         model_sess_id);
    if (remove) {
      LOG(INFO) << "Remove model session: " << model_sess_id;
      for (auto iter : session_info->backend_weights) {
        auto backend = GetBackend(iter.first);
        backend->UnloadModel(model_sess_id);
        update_backends.insert(backend);
      }
      session_table_.erase(model_sess_id);
    }
  }
  // Remove model sessions
  for (auto backend : update_backends) {
    backend->UpdateModelTableRpc();
  }
}

BackendDelegatePtr Scheduler::GetBackend(uint32_t node_id) {
  auto iter = backends_.find(node_id);
  if (iter == backends_.end()) {
    LOG(ERROR) << "Cannot find backend " << node_id;
    return nullptr;
  }
  return iter->second;
}

FrontendDelegatePtr Scheduler::GetFrontend(uint32_t node_id) {
  auto iter = frontends_.find(node_id);
  if (iter == frontends_.end()) {
    LOG(ERROR) << "Cannot find frontend " << node_id;
    return nullptr;
  }
  return iter->second;
}

FrontendDelegatePtr Scheduler::GetDispatcher(uint32_t node_id) {
  auto iter = dispatchers_.find(node_id);
  if (iter == dispatchers_.end()) {
    LOG(ERROR) << "Cannot find dispatcher " << node_id;
    return nullptr;
  }
  return iter->second;
}

void Scheduler::GetModelRoute(const std::string& model_sess_id,
                              ModelRouteProto* route) {
  route->set_model_session_id(model_sess_id);
  for (auto iter : session_table_.at(model_sess_id)->backend_weights) {
    auto backend_rate = route->add_backend_rate();
    backends_.at(iter.first)->GetInfo(backend_rate->mutable_info());
    backend_rate->set_throughput(iter.second);
  }
}

void Scheduler::FindBestBackend(const ModelSession& model_sess,
                                double request_rate,
                                const std::unordered_set<uint32_t>& skips,
                                BackendDelegatePtr* best_backend,
                                InstanceInfo* inst_info) {
  using ModelLoad = std::tuple<BackendDelegatePtr, InstanceInfo, double>;
  ModelLoad max_tp_load;
  ModelLoad max_occ_load;
  for (auto iter : backends_) {
    auto backend = iter.second;
    if (skips.find(backend->node_id()) != skips.end()) {
      continue;
    }
    if (!backend->IsAlive() || backend->workload_id() >= 0) {
      continue;
    }
    if (std::fabs(request_rate) < 1e-3 && !backend->IsIdle()) {
      continue;
    }
    InstanceInfo tmp_info;
    double occupancy;
    bool ret = backend->PrepareLoadModel(model_sess, request_rate, &tmp_info,
                                         &occupancy);
    if (!ret) {
      continue;
    }
    if (std::get<0>(max_tp_load) == nullptr ||
        tmp_info.throughput > std::get<1>(max_tp_load).throughput) {
      max_tp_load = std::make_tuple(backend, tmp_info, occupancy);
    }
    if (std::get<0>(max_occ_load) == nullptr ||
        occupancy > std::get<2>(max_occ_load)) {
      max_occ_load = std::make_tuple(backend, tmp_info, occupancy);
    }
  }
  if (std::fabs(request_rate) < 1e-3) {
    // for request rate = 0, return backend that provides highest throughput
    *best_backend = std::get<0>(max_tp_load);
    *inst_info = std::get<1>(max_tp_load);
  } else if (std::get<1>(max_tp_load).throughput < request_rate) {
    // If no backend can achieve request rate, return backend that provides
    // highest throughput
    *best_backend = std::get<0>(max_tp_load);
    *inst_info = std::get<1>(max_tp_load);
  } else {
    // Otherwise, return backend that has highest occupancy
    *best_backend = std::get<0>(max_occ_load);
    *inst_info = std::get<1>(max_occ_load);
  }
}

bool Scheduler::BeaconCheck() {
  std::lock_guard<std::mutex> lock(mutex_);
  // 1. Remove dead frontends
  std::vector<FrontendDelegatePtr> dead_frontends;
  for (auto iter : frontends_) {
    auto frontend = iter.second;
    if (frontend->IsAlive()) {
      continue;
    }
    dead_frontends.push_back(frontend);
  }
  for (auto frontend : dead_frontends) {
    frontends_.erase(frontend->node_id());
    std::time_t last_time = frontend->LastAliveTime();
    LOG(INFO) << "Remove frontend " << frontend->node_id()
              << ", last alive time: " << std::ctime(&last_time);
    RemoveFrontend(frontend);
  }

  // 2. Aggregate model session rps
  for (auto iter : session_table_) {
    const auto& model_sess_id = iter.first;
    auto session_info = iter.second;
    double rps = 0.;
    for (auto const& wk_iter : session_info->workloads) {
      rps += std::max(0., wk_iter.second->rate());
    }
    if (session_info->rps_history.size() > 0 || rps > 0) {
      // Don't push 0 in the begining
      session_info->rps_history.push_back(rps);
    }
    if (session_info->rps_history.size() > history_len_) {
      session_info->rps_history.pop_front();
    }
    VLOG(2) << "Model " << model_sess_id << " rps: " << rps
            << " req/s (avg over " << FLAGS_avg_interval << " seconds)";
  }

  // 3. Remove dead backend
  std::vector<BackendDelegatePtr> dead_backends;
  for (auto iter : backends_) {
    auto backend = iter.second;
    if (backend->IsAlive()) {
      continue;
    }
    dead_backends.push_back(backend);
  }
  // Reassign workload of dead backends
  for (auto backend : dead_backends) {
    RemoveBackend(backend);
  }
  for (auto backend : dead_backends) {
    std::time_t last_time = backend->LastAliveTime();
    LOG(INFO) << "Remove backend " << backend->node_id()
              << ", last alive time: " << std::ctime(&last_time);
    backends_.erase(backend->node_id());
  }

  // 4. Check if need to trigger epoch schedule
  bool trigger = false;
  for (auto iter : session_table_) {
    const auto& model_sess_id = iter.first;
    auto session_info = iter.second;
    if (session_info->rps_history.size() < history_len_) {
      continue;
    }
    double estimate_rps =
        std::max(session_info->rps_history[history_len_ - 1], 0.1);
    double throughput = session_info->TotalThroughput();
    if (estimate_rps < throughput * 0.8 || estimate_rps > throughput * 1.1) {
      trigger = true;
      break;
    }
  }
  return trigger;
}

void Scheduler::EpochSchedule() {
  std::lock_guard<std::mutex> lock(mutex_);
  std::unordered_set<std::shared_ptr<SessionInfo> > visited;
  std::unordered_set<std::shared_ptr<SessionInfo> > changed_sessions;
  std::vector<BackendDelegatePtr> overload_backends;

  VLOG(1) << "Epoch schedule";
  // 1. Adjust the GPU allocation based on the workload
  for (auto iter : session_table_) {
    auto const& model_sess_id = iter.first;
    auto session_info = iter.second;
    if (visited.count(session_info) > 0) {
      continue;
    }
    visited.insert(session_info);
    double throughput = session_info->TotalThroughput();
    // Compute the workload mean and std
    uint32_t n = session_info->rps_history.size();
    if (n < history_len_) {
      continue;
    }
    double rps_mean = 0., rps_std = 0.;
    for (double rps : session_info->rps_history) {
      rps_mean += rps;
    }
    rps_mean /= n;
    for (double rps : session_info->rps_history) {
      rps_std += (rps - rps_mean) * (rps - rps_mean);
    }
    rps_std = std::sqrt(rps_std / (n - 1));
    // double estimate_rps = std::max(
    //     session_info->rps_history[n - 1] + rps_std, 0.1);
    // double estimate_rps = std::max(rps_mean + rps_std, 0.1);
    double estimate_rps = std::max(session_info->rps_history[n - 1], 0.1);
    session_info->unassigned_workload = std::max(0., estimate_rps - throughput);
    VLOG(1) << model_sess_id << " estimate rps: " << estimate_rps
            << " (last: " << session_info->rps_history[n - 1]
            << ", mean: " << rps_mean << ", std: " << rps_std
            << "), throughput: " << throughput;

    if (estimate_rps < throughput * 0.97) {
      // Workload is smaller than throughput, can release some GPUs
      std::vector<std::pair<uint32_t, double> > adjust_backends;
      // Backends with static configured workload are still fixed
      for (auto iter : session_info->backend_weights) {
        auto backend = backends_.at(iter.first);
        if (backend->workload_id() >= 0) {
          estimate_rps -= iter.second;
        } else {
          adjust_backends.push_back(iter);
        }
      }
      // Sort the backends based on throughput
      std::sort(
          adjust_backends.begin(), adjust_backends.end(),
          [](std::pair<uint32_t, double> a, std::pair<uint32_t, double> b) {
            return a.second > b.second;
          });
      for (auto iter : adjust_backends) {
        if (estimate_rps < 1e-3) {
          auto backend = backends_.at(iter.first);
          backend->UnloadModel(model_sess_id);
          session_info->backend_weights.erase(iter.first);
        } else if (iter.second > estimate_rps) {
          auto backend = backends_.at(iter.first);
          double new_tp =
              backend->UpdateModelThroughput(model_sess_id, estimate_rps);
          session_info->backend_weights[iter.first] =
              backend->GetModelWeight(model_sess_id);
          estimate_rps -= new_tp;
        } else {
          estimate_rps -= iter.second;
        }
      }
      changed_sessions.insert(session_info);
    } else if (estimate_rps > throughput) {
      // Workload is larger than throughput, need to allocate more gpus
      std::vector<std::pair<uint32_t, double> > adjust_backends;
      // Backends with static configured workload are still fix
      for (auto iter : session_info->backend_weights) {
        auto backend = backends_.at(iter.first);
        if (backend->workload_id() >= 0) {
          estimate_rps -= iter.second;
        } else {
          adjust_backends.push_back(iter);
        }
      }
      // Second sort the backends based on throughput in descending order
      std::sort(
          adjust_backends.begin(), adjust_backends.end(),
          [](std::pair<uint32_t, double> a, std::pair<uint32_t, double> b) {
            return a.second > b.second;
          });
      for (auto iter : adjust_backends) {
        if (estimate_rps < 1e-3) {
          auto backend = backends_.at(iter.first);
          backend->UnloadModel(model_sess_id);
          session_info->backend_weights.erase(iter.first);
        } else {
          auto backend = backends_.at(iter.first);
          double new_tp =
              backend->UpdateModelThroughput(model_sess_id, estimate_rps);
          session_info->backend_weights[iter.first] =
              backend->GetModelWeight(model_sess_id);
          estimate_rps -= new_tp;
          if (backend->overload() && backend->Occupancy() > 1.05) {
            overload_backends.push_back(backend);
          }
        }
      }
      if (estimate_rps > 1e-3) {
        session_info->unassigned_workload = estimate_rps;
      } else {
        session_info->unassigned_workload = 0;
      }
      changed_sessions.insert(session_info);
    }
  }

  // 2. Adjust overloaded backends
  for (auto backend : overload_backends) {
    std::vector<std::pair<SessionGroup, double> > spillout;
    backend->SpillOutWorkload(&spillout);
    for (auto iter : spillout) {
      auto model_sess_id = ModelSessionToString(iter.first[0]);
      auto session_info = session_table_.at(model_sess_id);
      session_info->backend_weights.erase(backend->node_id());
      session_info->unassigned_workload += iter.second;
    }
  }

  // 3. Consolidate low utilization backends
  // ConsolidateBackends(&changed_sessions);

  // 4. Allocate the unassigned workloads to backends that still have space
  AllocateUnassignedWorkloads(&changed_sessions);

  // 5. Update model table to backends and model routes to frontends
  for (auto iter : backends_) {
    iter.second->UpdateModelTableRpc();
  }
  UpdateModelRoutes(changed_sessions);

  DisplayModelTable();
}

void Scheduler::AllocateUnassignedWorkloads(
    std::unordered_set<SessionInfoPtr>* changed_sessions,
    std::unordered_set<BackendDelegatePtr>* changed_backends) {
  // Sort unassigned workloads by request rate
  std::vector<SessionInfoPtr> unassigned_workloads;
  std::unordered_set<SessionInfoPtr> visited;
  for (auto iter : session_table_) {
    auto session_info = iter.second;
    if (visited.count(session_info) > 0) {
      continue;
    }
    visited.insert(session_info);
    if (session_info->unassigned_workload > 1e-3) {
      VLOG(1) << iter.first << " has unassigned workload "
              << session_info->unassigned_workload;
      unassigned_workloads.emplace_back(session_info);
    }
  }
  if (unassigned_workloads.empty()) {
    return;
  }
  std::sort(unassigned_workloads.begin(), unassigned_workloads.end(),
            [](SessionInfoPtr a, SessionInfoPtr b) {
              return a->unassigned_workload > b->unassigned_workload;
            });
  for (auto session_info : unassigned_workloads) {
    double request_rate = session_info->unassigned_workload;
    auto const& sessions = session_info->model_sessions;
    // LOG(INFO) << "Try to assign workload " << model_sess_id << ", " <<
    //     request_rate << " req/s";
    while (request_rate > 1e-3) {
      BackendDelegatePtr backend;
      InstanceInfo inst_info;
      FindBestBackend(sessions[0], request_rate, {}, &backend, &inst_info);
      if (backend == nullptr) {
        std::string model_sess_id = ModelSessionToString(sessions[0]);
        LOG(INFO) << "Unassigned workload " << model_sess_id << ", "
                  << request_rate << " req/s";
        break;
      }
      request_rate -= inst_info.throughput;
      backend->LoadModel(inst_info);
      for (int i = 1; i < sessions.size(); ++i) {
        backend->LoadPrefixModel(sessions[i], sessions[0]);
      }
      session_info->backend_weights.emplace(backend->node_id(),
                                            inst_info.GetWeight());
      changed_sessions->insert(session_info);
      if (changed_backends != nullptr) {
        changed_backends->insert(backend);
      }
    }
    if (std::fabs(request_rate) < 1e-3) request_rate = 0.;
    session_info->unassigned_workload = std::max(0., request_rate);
  }
}

void Scheduler::ConsolidateBackends(
    std::unordered_set<SessionInfoPtr>* changed_sessions) {
  std::vector<BackendDelegatePtr> backends;
  std::unordered_set<uint32_t> skip_backends;
  for (auto iter : backends_) {
    auto backend = iter.second;
    if (backend->Occupancy() == 0) {
      skip_backends.insert(backend->node_id());
    } else {
      backends.push_back(backend);
    }
  }
  while (!backends.empty()) {
    std::sort(backends.begin(), backends.end(),
              [](const BackendDelegatePtr& a, const BackendDelegatePtr& b) {
                return a->Occupancy() > b->Occupancy();
              });
    auto backend = backends.back();
    backends.pop_back();
    skip_backends.insert(backend->node_id());
    bool full = false;
    // changed_backends->insert(backend);
    for (auto inst_info : backend->GetModels()) {
      auto const& model_sess = inst_info->model_sessions[0];
      std::string model_sess_id = ModelSessionToString(model_sess);
      BackendDelegatePtr assign;
      InstanceInfo new_inst_info;
      FindBestBackend(model_sess, inst_info->workload, skip_backends, &assign,
                      &new_inst_info);
      if (assign == nullptr) {
        full = true;
        break;
      }
      backend->UnloadModel(model_sess_id);
      assign->LoadModel(new_inst_info);
      if (inst_info->model_sessions.size() > 1) {
        for (uint i = 1; i < inst_info->model_sessions.size(); ++i) {
          assign->LoadPrefixModel(inst_info->model_sessions[i], model_sess);
          backend->UnloadModel(
              ModelSessionToString(inst_info->model_sessions[i]));
        }
      }
      // changed_backends->insert(assign);
      auto session_info = session_table_.at(model_sess_id);
      session_info->backend_weights.erase(backend->node_id());
      session_info->backend_weights.emplace(assign->node_id(),
                                            new_inst_info.GetWeight());
      changed_sessions->insert(session_info);
      LOG(INFO) << "Move model " << model_sess_id << " from "
                << backend->node_id() << " to " << assign->node_id();
    }
    if (full) {
      break;
    }
  }
}

void Scheduler::UpdateModelRoutes(std::unordered_set<SessionInfoPtr> sessions) {
  std::unordered_map<uint32_t, ModelRouteUpdates> frontend_updates;
  ModelRouteUpdates dispatcher_update;
  for (auto session_info : sessions) {
    for (auto const& iter : session_info->session_subscribers) {
      for (auto frontend_id : iter.second) {
        if (frontend_updates.find(frontend_id) == frontend_updates.end()) {
          frontend_updates.emplace(frontend_id, ModelRouteUpdates());
        }
        GetModelRoute(iter.first,
                      frontend_updates.at(frontend_id).add_model_route());
      }
      // Also send all updates to the dispatcher
      GetModelRoute(iter.first, dispatcher_update.add_model_route());
    }
  }
  for (auto iter : frontend_updates) {
    auto frontend = GetFrontend(iter.first);
    if (frontend != nullptr) {
      frontend->UpdateModelRoutesRpc(iter.second);
    }
  }
  for (const auto& iter : dispatchers_) {
    iter.second->UpdateModelRoutesRpc(dispatcher_update);
  }
}

void Scheduler::DisplayModelTable() {
  std::unordered_set<uint32_t> used_backends;
  std::stringstream ss;
  for (auto iter : backends_) {
    auto backend = iter.second;
    double occ = backend->Occupancy();
    if (occ > 0) {
      used_backends.insert(backend->node_id());
      ss << "Backend " << backend->node_id() << ": " << occ << "\n";
    }
  }
  if (!used_backends.empty()) {
    VLOG(1) << "Total used GPUs: " << used_backends.size() << "\n" << ss.str();
    std::stringstream ss1;
    for (auto iter : session_table_) {
      auto const& model_sess_id = iter.first;
      auto session_info = iter.second;
      double total_gpu_share = 0.;
      ss1 << model_sess_id << ":";
      for (auto backend_iter : session_info->backend_weights) {
        auto backend = GetBackend(backend_iter.first);
        double share = backend->GetModelGPUShare(model_sess_id);
        total_gpu_share += share;
        ss1 << " " << backend_iter.first << "/" << backend_iter.second << "/"
            << share;
        used_backends.insert(backend_iter.first);
      }
      ss1 << ", total share: " << total_gpu_share << "\n";
    }
    VLOG(1) << "Model table: \n" << ss1.str();
  }
}

}  // namespace scheduler
}  // namespace nexus

max_stars_repo_name: dengwxn/nexus

max_issues_repo_name: dengwxn/nexus

max_forks_repo_name: dengwxn/nexus


text:
/*
 Copyright (c) 2010, The Cinder Project
 All rights reserved.

 Copyright (c) Microsoft Open Technologies, Inc. All rights reserved.

 This code is designed for use with the Cinder C++ library, http://libcinder.org

 Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 the following conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and
	the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
	the following disclaimer in the documentation and/or other materials provided with the distribution.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGE.
*/

#include "cinder/Xml.h"
#include "cinder/Utilities.h"
#include <boost/algorithm/string.hpp>

#include "rapidxml/rapidxml.hpp"
#include "rapidxml/rapidxml_print.hpp"

using namespace std;

namespace cinder {

void parseItem( const rapidxml::xml_node<> &node, XmlTree *parent, XmlTree *result, const XmlTree::ParseOptions &parseOptions );

namespace {
bool tagsMatch( const std::string &tag1, const std::string &tag2, bool caseSensitive )
{
	if( caseSensitive && ( tag1 == tag2 ) )
		return true;
	else if( ( ! caseSensitive ) && ( boost::iequals( tag1, tag2 ) ) )
		return true;
	else
		return false;
}
} // anonymous namespace

XmlTree::ConstIter::ConstIter( const Container *sequence )
{
	mSequenceStack.push_back( sequence );
	mIterStack.push_back( sequence->begin() );
}

XmlTree::ConstIter::ConstIter( const Container *sequence, Container::const_iterator iter )
{
	mSequenceStack.push_back( sequence );
	mIterStack.push_back( iter );
}

XmlTree::ConstIter::ConstIter( const XmlTree &root, const string &filterPath, bool caseSensitive, char separator )
	: mCaseSensitive( caseSensitive )
{
	mFilter = split( filterPath, separator );

	// we ignore a leading separator so that "/one/two" is equivalent to "one/two"
	if( ( ! filterPath.empty() ) && ( filterPath[0] == separator ) && ( ! mFilter.empty() ) )
		mFilter.erase( mFilter.begin() );

	if( mFilter.empty() ) { // empty filter means nothing matches
		setToEnd( &root.getChildren() );
		return;
	}

	for( vector<string>::const_iterator filterComp = mFilter.begin(); filterComp != mFilter.end(); ++filterComp ) {
		if( mIterStack.empty() ) // first item
			mSequenceStack.push_back( &root.getChildren() );
		else
			mSequenceStack.push_back( &(*mIterStack.back())->getChildren() );

		Container::const_iterator child = findNextChildNamed( *mSequenceStack.back(), mSequenceStack.back()->begin(), *filterComp, mCaseSensitive );
		if( child != (mSequenceStack.back())->end() )
			mIterStack.push_back( child );
		else { // failed to find an item that matches this part of the filter; mark as finished and return
			setToEnd( &root.getChildren() );
			return;
		}
	}
}

// sets the iterator to be pointing to the end, meaning iteration is done
void XmlTree::ConstIter::setToEnd( const Container *seq )
{
	mSequenceStack.clear();
	mSequenceStack.push_back( seq );
	mIterStack.clear();
	mIterStack.push_back( seq->end() );
}

bool XmlTree::ConstIter::isDone() const
{
	return ( mSequenceStack.size() == 1 ) && ( mIterStack.back() == mSequenceStack[0]->end() );
}

void XmlTree::ConstIter::increment()
{
	++mIterStack.back();

	if( ! mFilter.empty() ) {

		bool found = false;
		do {
			Container::const_iterator next = findNextChildNamed( *mSequenceStack.back(), mIterStack.back(), mFilter[mSequenceStack.size()-1], mCaseSensitive );
			if( next == mSequenceStack.back()->end() ) { // we've finished this part of the sequence stack
				if( mSequenceStack.size() > 1 ) { // we might already be done, in which case incrementing would be bad
					mIterStack.pop_back();
					mSequenceStack.pop_back();
					++mIterStack.back(); // next in the new top, which was formerly top-1
				}
				else
					mIterStack[0] = mSequenceStack.back()->end(); // all done
			}
			else if( mSequenceStack.size() < mFilter.size() ) { // we're not on a leaf, so push this onto the stack
				mIterStack[mIterStack.size()-1] = next;
				mSequenceStack.push_back( &(*next)->getChildren() );
				mIterStack.push_back( (*next)->getChildren().begin() );
			}
			else {
				mIterStack[mIterStack.size()-1] = next;
				found = true;
			}
		} while( ( ! isDone() ) && ( ! found ) );
	}
}

XmlTree::Container::const_iterator XmlTree::findNextChildNamed( const Container &sequence, Container::const_iterator firstCandidate, const string &searchTag, bool caseSensitive )
{
	Container::const_iterator result = firstCandidate;
	while( result != sequence.end() ) {
		if( tagsMatch( (*result)->getTag(), searchTag, caseSensitive ) )
			break;
		else
			++result;
	}
	return result;
}

XmlTree::XmlTree( const XmlTree &rhs )
	: mNodeType( rhs.mNodeType ), mTag( rhs.mTag ), mValue( rhs.mValue ), mDocType( rhs.mDocType ),
	 mParent( 0 ), mAttributes( rhs.mAttributes )
{
	for( XmlTree::ConstIter childIt = rhs.begin(); childIt != rhs.end(); ++childIt ) {
		mChildren.push_back( unique_ptr<XmlTree>( new XmlTree( *childIt ) ) );
		mChildren.back()->mParent = this;
	}
}

XmlTree& XmlTree::operator=( const XmlTree &rhs )
{
	mNodeType = rhs.mNodeType;
	mTag = rhs.mTag;
	mValue = rhs.mValue;
	mDocType = rhs.mDocType;
	mParent = 0;
	mAttributes = rhs.mAttributes;

	mChildren.clear();

	for( XmlTree::ConstIter childIt = rhs.begin(); childIt != rhs.end(); ++childIt ) {
		mChildren.push_back( unique_ptr<XmlTree>( new XmlTree( *childIt ) ) );
		mChildren.back()->mParent = this;
	}

	return *this;
}

XmlTree::XmlTree( const std::string &xmlString, ParseOptions parseOptions )
{
	std::string strCopy( xmlString );
	rapidxml::xml_document<> doc;    // character type defaults to char
	if( parseOptions.getParseComments() )
		doc.parse<rapidxml::parse_comment_nodes | rapidxml::parse_doctype_node>( &strCopy[0] );
	else
		doc.parse<rapidxml::parse_doctype_node>( &strCopy[0] );
	parseItem( doc, NULL, this, parseOptions );
	setNodeType( NODE_DOCUMENT ); // call this after parse - constructor replaces it
}

void parseItem( const rapidxml::xml_node<> &node, XmlTree *parent, XmlTree *result, const XmlTree::ParseOptions &options )
{
	*result = XmlTree( node.name(), node.value(), parent );
	for( const rapidxml::xml_node<> *item = node.first_node(); item; item = item->next_sibling() ) {
		XmlTree::NodeType type;
		switch( item->type() ) {
			case rapidxml::node_element:
				type = XmlTree::NODE_ELEMENT;
			break;
			case rapidxml::node_cdata: {
				if( options.getCollapseCData() ) {
					result->setValue( result->getValue() + item->value() );
					continue;
				}
				else {
					type = XmlTree::NODE_CDATA;
				}
			}
			break;
			case rapidxml::node_comment:
				type = XmlTree::NODE_COMMENT;
			break;
			case rapidxml::node_doctype: {
				result->setDocType( item->value() );
				continue;
			}
			case rapidxml::node_data: {
				if( ! options.getIgnoreDataChildren() )
					type = XmlTree::NODE_DATA;
				else
					continue;
			}
			break;
			default:
				continue;
		}

		result->getChildren().push_back( unique_ptr<XmlTree>( new XmlTree ) );
		parseItem( *item, result, result->getChildren().back().get(), options );
		result->getChildren().back()->setNodeType( type );
	}

	for( rapidxml::xml_attribute<> *attr = node.first_attribute(); attr; attr = attr->next_attribute() )
		result->getAttributes().push_back( XmlTree::Attr( result, attr->name(), attr->value() ) );
}

void XmlTree::loadFromDataSource( DataSourceRef dataSource, XmlTree *result, const XmlTree::ParseOptions &parseOptions )
{
	auto buf = dataSource->getBuffer();
	size_t dataSize = buf->getSize();
	unique_ptr<char[]> bufString( new char[dataSize+1] );
	memcpy( bufString.get(), buf->getData(), buf->getSize() );
	bufString.get()[dataSize] = 0;
	rapidxml::xml_document<> doc;    // character type defaults to char
	if( parseOptions.getParseComments() )
		doc.parse<rapidxml::parse_comment_nodes | rapidxml::parse_doctype_node>( bufString.get() );
	else
		doc.parse<rapidxml::parse_doctype_node>( bufString.get() );
	parseItem( doc, NULL, result, parseOptions );
	result->setNodeType( NODE_DOCUMENT ); // call this after parse - constructor replaces it
}

bool XmlTree::hasChild( const string &relativePath, bool caseSensitive, char separator ) const
{
	return getNodePtr( relativePath, caseSensitive, separator ) != NULL;
}

const XmlTree& XmlTree::getChild( const string &relativePath, bool caseSensitive, char separator ) const
{
	XmlTree* child = getNodePtr( relativePath, caseSensitive, separator );
	if( child )
		return *child;
	else
		throw ExcChildNotFound( *this, relativePath );
}

XmlTree& XmlTree::getChild( const string &relativePath, bool caseSensitive, char separator )
{
	XmlTree* child = getNodePtr( relativePath, caseSensitive, separator );
	if( child )
		return *child;
	else
		throw ExcChildNotFound( *this, relativePath );
}

const XmlTree::Attr& XmlTree::getAttribute( const string &attrName ) const
{
	for( list<Attr>::const_iterator attrIt = mAttributes.begin(); attrIt != mAttributes.end(); ++attrIt )
		if( attrIt->getName() == attrName )
			return *attrIt;
	throw ExcAttrNotFound( *this, attrName );
}

XmlTree& XmlTree::setAttribute( const std::string &attrName, const std::string &value )
{
	list<Attr>::iterator atIt;
	for( atIt = mAttributes.begin(); atIt != mAttributes.end(); ++atIt )
		if( atIt->getName() == attrName )
			break;

	if( atIt == mAttributes.end() )
		mAttributes.push_back( Attr( this, attrName, value ) );
	else
		atIt->setValue( value );

	return *this;
}

bool XmlTree::hasAttribute( const std::string &attrName ) const
{
	for( list<Attr>::const_iterator atIt = mAttributes.begin(); atIt != mAttributes.end(); ++atIt )
		if( atIt->getName() == attrName )
			return true;

	return false;
}

string	XmlTree::getPath( char separator ) const
{
	string result;

	const XmlTree *node = this;
	while( node ) {
		string nodeName = node->getTag();
		if( node != this )
			nodeName += separator;
		result = nodeName + result;
		node = node->mParent;
	}

	return result;
}

void XmlTree::push_back( const XmlTree &newChild )
{
	mChildren.push_back( unique_ptr<XmlTree>( new XmlTree( newChild ) ) );
	mChildren.back()->mParent = this;
}

XmlTree* XmlTree::getNodePtr( const string &relativePath, bool caseSensitive, char separator ) const
{
	XmlTree *curNode = const_cast<XmlTree*>( this );

	vector<string> pathComponents = split( relativePath, separator );
	for( vector<string>::const_iterator pathIt = pathComponents.begin(); pathIt != pathComponents.end(); ++pathIt ) {
		if( pathIt->empty() )
			continue;
		Container::const_iterator node = XmlTree::findNextChildNamed( curNode->getChildren(), curNode->getChildren().begin(), *pathIt, caseSensitive )	;
		if( node != curNode->getChildren().end() )
			curNode = const_cast<XmlTree*>( node->get() );
		else
			return 0;
	}

	return curNode;
}

void XmlTree::appendRapidXmlNode( rapidxml::xml_document<char> &doc, rapidxml::xml_node<char> *parent ) const
{
	rapidxml::node_type type;
	switch( getNodeType() ) {
		case XmlTree::NODE_ELEMENT: type = rapidxml::node_element; break;
		case XmlTree::NODE_COMMENT: type = rapidxml::node_comment; break;
		case XmlTree::NODE_CDATA: type = rapidxml::node_cdata; break;
		case XmlTree::NODE_DATA: type = rapidxml::node_data; break;

		default: throw ExcUnknownNodeType();
	}
	rapidxml::xml_node<char> *node = 0;
	if( type == rapidxml::node_data ) {
		node = doc.allocate_node( type, NULL, doc.allocate_string( getValue().c_str() ) );
	}
	else if( type == rapidxml::node_comment ) {
		node = doc.allocate_node( type, doc.allocate_string( getTag().c_str() ), doc.allocate_string( getValue().c_str() ) );
	}
	else {
		node = doc.allocate_node( type, doc.allocate_string( getTag().c_str() ), NULL );
		if( ! getValue().empty() )
			node->append_node( doc.allocate_node( rapidxml::node_data, NULL, doc.allocate_string( getValue().c_str() ) ) );
	}
	parent->append_node( node );

	for( list<Attr>::const_iterator attrIt = mAttributes.begin(); attrIt != mAttributes.end(); ++attrIt )
		node->append_attribute( doc.allocate_attribute( doc.allocate_string( attrIt->getName().c_str() ), doc.allocate_string( attrIt->getValue().c_str() ) ) );

	for( Container::const_iterator childIt = mChildren.begin(); childIt != mChildren.end(); ++childIt )
		(*childIt)->appendRapidXmlNode( doc, node );
}

shared_ptr<rapidxml::xml_document<char> > XmlTree::createRapidXmlDoc( bool createDocument ) const
{
	shared_ptr<rapidxml::xml_document<char> > result( new rapidxml::xml_document<>() );
	if( isDocument() || createDocument ) {
		rapidxml::xml_node<char> *declarationNode = result->allocate_node( rapidxml::node_declaration, "", "" );
		result->append_node( declarationNode );
		if( isDocument() && ( ! mDocType.empty() ) )
			result->append_node( result->allocate_node( rapidxml::node_doctype, "", result->allocate_string( mDocType.c_str() ) ) );

		if( isDocument() ) {
			for( Container::const_iterator childIt = mChildren.begin(); childIt != mChildren.end(); ++childIt )
				(*childIt)->appendRapidXmlNode( *result, result.get() );
		}
		else {
			appendRapidXmlNode( *result, result.get() );
		}

		declarationNode->append_attribute( result->allocate_attribute( "version", "1.0" ) );
		declarationNode->append_attribute( result->allocate_attribute( "encoding", "utf-8" ) );
	}
	else
		appendRapidXmlNode( *result, result.get() );
	return result;
}

ostream& operator<<( ostream &out, const XmlTree &xml )
{
	shared_ptr<rapidxml::xml_document<char> > doc = xml.createRapidXmlDoc( false );
	return rapidxml::print( out, *doc, 0 );
}

void XmlTree::write( DataTargetRef target, bool createDocument )
{
	// this could do with a more efficient implementation
	shared_ptr<rapidxml::xml_document<char> > doc = createRapidXmlDoc( createDocument );
	OStreamRef os = target->getStream();
	std::ostringstream ss;
	ss << *doc;
	os->writeData( ss.str().c_str(), ss.str().length() );
}

XmlTree::ExcChildNotFound::ExcChildNotFound( const XmlTree &node, const string &childPath ) throw()
{
#if (defined (CINDER_MSW ) || defined( CINDER_WINRT ))
	sprintf_s( mMessage, "Could not find child: %s for node: %s", childPath.c_str(), node.getPath().c_str() );
#else
	sprintf( mMessage, "Could not find child: %s for node: %s", childPath.c_str(), node.getPath().c_str() );
#endif
}

XmlTree::ExcAttrNotFound::ExcAttrNotFound( const XmlTree &node, const string &attrName ) throw()
{
#if (defined (CINDER_MSW ) || defined( CINDER_WINRT ))
	sprintf_s( mMessage, "Could not find attribute: %s for node: %s", attrName.c_str(), node.getPath().c_str() );
#else
	sprintf( mMessage, "Could not find attribute: %s for node: %s", attrName.c_str(), node.getPath().c_str() );
#endif
}

} // namespace cinder

ext: cpp

max_stars_repo_name: migeran/Cinder

max_issues_repo_name: migeran/Cinder

max_forks_repo_name: migeran/Cinder


text:
/* test_counter_detail.cpp
 *
 * Copyright (c) 2014 M.A. (Thijs) van den Berg
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */
#include <istream>
#include <ostream>
#include <iomanip>
#include <string>
#include <algorithm> // std::min

#include <boost/multiprecision/cpp_int.hpp>

#include <boost/random/threefry.hpp>
#include <boost/random/philox.hpp>
#include <boost/random/counter_based_engine.hpp>

#define BOOST_TEST_MODULE counter_engine_counter
#include <boost/test/included/unit_test.hpp>

#define BOOST_COUNTER_BASED_ENGINE_RESULT_TYPE uint64_t
#define BOOST_PSEUDO_RANDOM_FUNCTION boost::random::philox<4, uint64_t>
#define BOOST_COUNTER_BASED_ENGINE_CTRBITS 128u

typedef boost::random::counter_based_engine<BOOST_COUNTER_BASED_ENGINE_RESULT_TYPE, BOOST_PSEUDO_RANDOM_FUNCTION, BOOST_COUNTER_BASED_ENGINE_CTRBITS> engine_t;
#define BOOST_RANDOM_URNG engine_t

typedef engine_t::domain_type::value_type dvalue_type;
static const unsigned ctrbits = BOOST_RANDOM_URNG::counter_bits;

static const unsigned dvalbits = std::numeric_limits<dvalue_type>::digits;
static const unsigned ndomain = BOOST_RANDOM_URNG::domain_traits::Nbits / dvalbits;

static const unsigned nrange = engine_t::results_per_counter();

typedef engine_t::key_type::value_type kvalue_type;
static const unsigned kvalbits = std::numeric_limits<kvalue_type>::digits;
static const unsigned nkey = engine_t::key_traits::Nbits/kvalbits;

using namespace boost::multiprecision;

/*
 * Extract the "sample counter" value from a counter based engine.
 * "sample_counter" tell the numer of samples that are drawn from the engine.
 * It has a value of 1 after the first draw and increases with 1 after each draw.
 * If each encryption round generated S samples then this value is S * (2^counter_bits -1) + current_pos
 */
uint1024_t get_sample_counter(BOOST_RANDOM_URNG& urng)
{
    uint1024_t counter, digit;
    unsigned subcounter;

    std::stringstream ss;
    ss << urng;
    ss >> subcounter;

    for (unsigned int i=0; i<ndomain; ++i) {
        ss >> digit;
        counter <<= dvalbits;
        counter += digit;
    }

    counter &= (uint1024_t(1)<<ctrbits) - 1;
    counter = counter * nrange + subcounter;

    //std::cout << "counter=" <<  counter << " (" << os.str() << ")" << std::endl;
    return counter;
}

BOOST_AUTO_TEST_CASE(test_overflow)
{
    engine_t urng;
    engine_t urng0;

    // use 'private' knowledge of the external
    // stream representation to initialize a
    // nearly maximal counter.
    std::stringstream ss;
    ss << 0;
    for(size_t i=0; i<ndomain; ++i)
        ss << ' ' << ~dvalue_type(0);
    for(size_t i=0; i<nkey; ++i)
        ss << ' ' << 0;
    ss >> urng0;
    BOOST_CHECK(ss);

    // Run the urng right up to the edge..
    urng = urng0;
    for(unsigned i=0; i<nrange; ++i){
        urng();
        BOOST_CHECK_NE(urng, urng0);
    }
    // Verify that it throws when we push over the edge
    BOOST_CHECK_THROW(urng(), std::invalid_argument);

    // Try again with discard:
    urng = urng0;
    urng.discard(nrange);
    BOOST_CHECK_NE(urng, urng0);
    BOOST_CHECK_THROW(urng(), std::invalid_argument);

    // Make sure that discard itself throws if it pushes us over the
    // edge.
    urng = urng0;
    BOOST_CHECK_THROW(urng.discard(nrange+1), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(test_counter_detail)
{
    uint1024_t expected,actual,discards;
    BOOST_RANDOM_URNG urng;

    // initial check
    urng();                                 // generate a single random value to trigger initialisation
    expected = uint1024_t(1);               // we expect the sample counter to now be at 1
    actual = get_sample_counter(urng);
    BOOST_CHECK_EQUAL(expected, actual);

    // draw 300 samples
    for (unsigned i=0; i<300; ++i) {
        urng();
        ++expected;
        actual = get_sample_counter(urng);
        BOOST_CHECK_EQUAL(expected, actual);
    }

    // do 300 big discards
    for (unsigned i=0; i<300; ++i) {
        discards = uint1024_t(0xFFFFFFFFFFFFFFFF);
        expected += discards;
        urng.discard(0xFFFFFFFFFFFFFFFF);
        actual = get_sample_counter(urng);
        BOOST_CHECK_EQUAL(expected, actual);
    }
}


text:
/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "tudat/basics/utilities.h"
#include "tudat/basics/testMacros.h"
#include "tudat/basics/basicTypedefs.h"
#include "tudat/io/basicInputOutput.h"
#include "tudat/io/matrixTextFileReader.h"

#include "tudat/math/statistics/basicStatistics.h"
#include "tudat/math/filters/extendedKalmanFilter.h"
#include "tudat/math/integrators/createNumericalIntegrator.h"

#include "tudat/math/filters/controlClass.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_extended_kalman_filter )

// Functions for extended Kalman filter.
Eigen::Vector2d stateFunction1( const double time, const Eigen::Vector2d& state, const Eigen::Vector2d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector2d stateDerivative;
    stateDerivative[ 0 ] = state[ 1 ] * std::pow( std::cos( state[ 0 ] ), 3 );
    stateDerivative[ 1 ] = std::sin( state[ 0 ] );
    return stateDerivative;
}
Eigen::Vector1d measurementFunction1( const double time, const Eigen::Vector2d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = std::pow( state[ 0 ], 3 );
    return measurement;
}
Eigen::Matrix2d stateJacobianFunction1( const double time, const Eigen::Vector2d& state, const Eigen::Vector2d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Matrix2d stateJacobian = Eigen::Matrix2d::Zero( );
    stateJacobian( 0, 0 ) = - 3.0 * state[ 1 ] * std::pow( std::cos( state[ 0 ] ), 2 ) * std::sin( state[ 0 ] );
    stateJacobian( 0, 1 ) = std::pow( std::cos( state[ 0 ] ), 3 );
    stateJacobian( 1, 0 ) = std::cos( state[ 0 ] );
    return stateJacobian;
}
Eigen::RowVector2d measurementJacobianFunction1( const double time, const Eigen::Vector2d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::RowVector2d measurementJacobian;
    measurementJacobian[ 0 ] = 3.0 * std::pow( state[ 0 ], 2 );
    return measurementJacobian;
}

// Test implementation of extended Kalman filter class.
BOOST_AUTO_TEST_CASE( testExtendedKalmanFilterFirstCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.01;
    const unsigned int numberOfTimeSteps = 1000;

    Eigen::Vector2d initialStateVector;
    initialStateVector[ 0 ] = 3.0;
    initialStateVector[ 1 ] = -0.3;

    Eigen::Vector2d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 10.0;
    initialEstimatedStateVector[ 1 ] = -3.0;

    Eigen::Matrix2d initialEstimatedStateCovarianceMatrix = Eigen::Matrix2d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = 100.0;
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 100.0;

    // Set system and measurement uncertainty
    Eigen::Matrix2d systemUncertainty = Eigen::Matrix2d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = 100.0;
    systemUncertainty( 1, 1 ) = 100.0;
    measurementUncertainty[ 0 ] = 100.0;

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 2 > > control =
            std::make_shared< ControlWrapper< double, double, 2 > >(
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Vector2d::Zero( ); } );

    // Create extended Kalman filter object
    KalmanFilterDoublePointer extendedFilter = std::make_shared< ExtendedKalmanFilterDouble >(
                std::bind( &stateFunction1, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 2 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction1, std::placeholders::_1, std::placeholders::_2 ),
                std::bind( &stateJacobianFunction1, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 2 >::getCurrentControlVector, control ) ),
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Matrix2d::Identity( ); },
                std::bind( &measurementJacobianFunction1, std::placeholders::_1, std::placeholders::_2 ),
                [ & ]( const double, const Eigen::Vector2d& ){ return Eigen::Vector1d::Identity( ); },
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                    "/ekfSystemNoise1.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                         "/ekfMeasurementNoise1.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector2d currentActualStateVector = initialStateVector;
    Eigen::Vector2d currentControlVector = Eigen::Vector2d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector2d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction1( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction1( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, extendedFilter->getCurrentStateEstimate( ) );

        // Update filter
        extendedFilter->updateFilter( currentMeasurementVector );
        currentTime = extendedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << extendedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector2d expectedFinalState;
    expectedFinalState << 4.972005968275564, -18.516511373319734;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( extendedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

// Constant parameters for example
const double gravitationalParameter = 32.2;

// Functions for extended Kalman filter.
Eigen::Vector3d stateFunction2( const double time, const Eigen::Vector3d& state, const Eigen::Vector3d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Vector3d stateDerivative = Eigen::Vector3d::Zero( );
    stateDerivative[ 0 ] = state[ 1 ];
    stateDerivative[ 1 ] = 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 2.0 * state[ 2 ] ) - gravitationalParameter;
    return stateDerivative;
}
Eigen::Vector1d measurementFunction2( const double time, const Eigen::Vector3d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    Eigen::Vector1d measurement;
    measurement[ 0 ] = state[ 0 ];
    return measurement;
}
Eigen::Matrix3d stateJacobianFunction2( const double time, const Eigen::Vector3d& state, const Eigen::Vector3d& control )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( control );
    Eigen::Matrix3d stateJacobian = Eigen::Matrix3d::Zero( );
    stateJacobian( 0, 1 ) = 1.0;
    stateJacobian( 1, 0 ) = - 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 44000.0 * state[ 2 ] );
    stateJacobian( 1, 1 ) = 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            state[ 1 ] / state[ 2 ];
    stateJacobian( 1, 2 ) = - 0.0034 * gravitationalParameter * std::exp( - state[ 0 ] / 22000.0 ) *
            std::pow( state[ 1 ], 2 ) / ( 2.0 * std::pow( state[ 2 ], 2 ) );
    return stateJacobian;
}
Eigen::RowVector3d measurementJacobianFunction2( const double time, const Eigen::Vector3d& state )
{
    TUDAT_UNUSED_PARAMETER( time );
    TUDAT_UNUSED_PARAMETER( state );
    Eigen::RowVector3d measurementJacobian = Eigen::RowVector3d::Zero( );
    measurementJacobian[ 0 ] = 1.0;
    return measurementJacobian;
}

// Test implementation of extended Kalman filter class.
BOOST_AUTO_TEST_CASE( testExtendedKalmanFilterSecondCase )
{
    using namespace tudat::filters;

    // Set initial conditions
    const double initialTime = 0.0;
    const double timeStep = 0.1;
    const unsigned int numberOfTimeSteps = 300;

    Eigen::Vector3d initialStateVector;
    initialStateVector[ 0 ] = 200000.0;
    initialStateVector[ 1 ] = -6000.0;
    initialStateVector[ 2 ] = 500.0;

    Eigen::Vector3d initialEstimatedStateVector;
    initialEstimatedStateVector[ 0 ] = 200025.0;
    initialEstimatedStateVector[ 1 ] = -6150.0;
    initialEstimatedStateVector[ 2 ] = 800.0;

    Eigen::Matrix3d initialEstimatedStateCovarianceMatrix = Eigen::Matrix3d::Zero( );
    initialEstimatedStateCovarianceMatrix( 0, 0 ) = std::pow( 1000.0, 2 );
    initialEstimatedStateCovarianceMatrix( 1, 1 ) = 20000.0;
    initialEstimatedStateCovarianceMatrix( 2, 2 ) = std::pow( 300.0, 2 );

    // Set system and measurement uncertainty
    Eigen::Matrix3d systemUncertainty = Eigen::Matrix3d::Zero( );
    Eigen::Vector1d measurementUncertainty = Eigen::Vector1d::Zero( );
    systemUncertainty( 0, 0 ) = std::pow( 100.0, 2 );
    systemUncertainty( 1, 1 ) = std::pow( 10.0, 2 );
    systemUncertainty( 2, 2 ) = std::pow( 1.0, 2 );
    measurementUncertainty[ 0 ] = std::pow( 25.0, 2 );

    // Set integrator settings
    std::shared_ptr< numerical_integrators::IntegratorSettings< > > integratorSettings =
            std::make_shared< numerical_integrators::IntegratorSettings< > > (
                numerical_integrators::euler, initialTime, timeStep );

    // Create control class
    std::shared_ptr< ControlWrapper< double, double, 3 > > control =
            std::make_shared< ControlWrapper< double, double, 3 > >(
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Vector3d::Zero( ); } );

    // Create extended Kalman filter object
    KalmanFilterDoublePointer extendedFilter = std::make_shared< ExtendedKalmanFilterDouble >(
                std::bind( &stateFunction2, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 3 >::getCurrentControlVector, control ) ),
                std::bind( &measurementFunction2, std::placeholders::_1, std::placeholders::_2 ),
                std::bind( &stateJacobianFunction2, std::placeholders::_1, std::placeholders::_2,
                             std::bind( &ControlWrapper< double, double, 3 >::getCurrentControlVector, control ) ),
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Matrix3d::Identity( ); },
                std::bind( &measurementJacobianFunction2, std::placeholders::_1, std::placeholders::_2 ),
                [ & ]( const double, const Eigen::Vector3d& ){ return Eigen::Vector1d::Identity( ); },
                systemUncertainty, measurementUncertainty, timeStep,
                initialTime, initialEstimatedStateVector, initialEstimatedStateCovarianceMatrix,
                integratorSettings );

    // Load noise from file
    Eigen::MatrixXd systemNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                    "/ekfSystemNoise2.dat" );
    Eigen::MatrixXd measurementNoise = input_output::readMatrixFromFile( tudat::paths::getTudatTestDataPath( ) +
                                                                         "/ekfMeasurementNoise2.dat" );

    // Loop over each time step
    const bool showProgress = false;
    double currentTime = initialTime;
    Eigen::Vector3d currentActualStateVector = initialStateVector;
    Eigen::Vector3d currentControlVector = Eigen::Vector3d::Zero( );
    Eigen::Vector1d currentMeasurementVector;
    std::map< double, Eigen::Vector3d > actualStateVectorHistory;
    std::map< double, Eigen::Vector1d > measurementVectorHistory;
    actualStateVectorHistory[ initialTime ] = initialStateVector;
    for ( unsigned int i = 0; i < numberOfTimeSteps; i++ )
    {
        // Compute actual values and perturb them
        currentActualStateVector += ( stateFunction2( currentTime, currentActualStateVector, currentControlVector ) +
                                      systemNoise.col( i ) ) * timeStep;
        currentMeasurementVector = measurementFunction2( currentTime, currentActualStateVector ) + measurementNoise.col( i );

        // Update control class
        control->setCurrentControlVector( currentTime, extendedFilter->getCurrentStateEstimate( ) );

        // Update filter
        extendedFilter->updateFilter( currentMeasurementVector );
        currentTime = extendedFilter->getCurrentTime( );

        // Store values
        actualStateVectorHistory[ currentTime ] = currentActualStateVector;
        measurementVectorHistory[ currentTime ] = currentMeasurementVector;

        // Print progress
        if ( showProgress )
        {
            std::cout << "Time: " << currentTime << std::endl
                      << "Measurement: " << currentMeasurementVector.transpose( ) << std::endl
                      << "Estimated State: " << extendedFilter->getCurrentStateEstimate( ).transpose( ) << std::endl << std::endl;
        }
    }

    // Check that final state is as expected
    Eigen::Vector3d expectedFinalState = Eigen::Vector3d::Zero( );
    expectedFinalState << 25202.174591778028, -3327.344292984541, 498.97471055815663;
    for ( int i = 0; i < expectedFinalState.rows( ); i++ )
    {
        BOOST_CHECK_SMALL( extendedFilter->getCurrentStateEstimate( )[ i ] - expectedFinalState[ i ], 1.0e-10 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


text:
#include <boost/filesystem.hpp>

#include <log4cpp/FileAppender.hh>
#include <log4cpp/OstreamAppender.hh>
#include <log4cpp/BasicLayout.hh>

#include "config.hpp"
#include "ProgramInterpreter.hpp"
#include "dataconnection.hpp"

using namespace std;
using namespace interp;
using namespace log4cpp;
using namespace boost;
using namespace enc;
using namespace fctr;
namespace files = boost::filesystem;

//////////////////////////////////////////
// Fonction d'utilisation
//////////////////////////////////////////

void usage(const char* exe)
{
  cout << "usage :" << endl;
  cout << "\t" << files::path(exe).filename().string() << " <file> -base <base> -v <log>" << endl << endl;
  cout << "\t<file>       : UTF-16LE input file" << endl;
  cout << "\t<log>        : log level" << endl;
  cout << "\t<base>       : database name" << endl << endl;
}

//////////////////////////////////////////
// Fonction d'initialisation du log
//////////////////////////////////////////

void initialize_log(const char* name, int verbose, string const& file_log = "")
{
    // 1 instantiate an appender object that
    // will append to a log file
    Appender* app;
    if (file_log.empty())
        app = new OstreamAppender(name, &std::cout);
    else
        app = new FileAppender(name, file_log);

    // 2. Instantiate a layout object
    // Two layouts come already available in log4cpp
    // unless you create your own.
    // BasicLayout includes a time stamp
    Layout* layout = new BasicLayout();

    // 3. attach the layout object to the
    // appender object
    app->setLayout(layout);

    // 4. Instantiate the category object
    // you may extract the root category, but it is
    // usually more practical to directly instance
    // a child category
    Category * main_cat = &Category::getInstance(name);

    // 5. Step 1
    // an Appender when added to a category becomes
    // an additional output destination unless
    // Additivity is set to false when it is false,
    // the appender added to the category replaces
    // all previously existing appenders
    main_cat->setAdditivity(false);

    // 5. Step 2
    // this appender becomes the only one
    main_cat->setAppender(app);

    // 6. Set up the priority for the category
    // and is given INFO priority
    // attempts to log DEBUG messages will fail
    main_cat->setPriority( verbose*100 );
}

int main(int argc, char *argv[])
{
    string filename;
    string base;
    int log = 7;
    if ( argc==6 )
    {
        int i = 1;
        while (i < 6)
        {
            if (0 == strcmp("-base", argv[i]))
            {
                base = argv[++i];
            }
            else if (0 == strcmp("-v", argv[i]))
            {
                log = atoi(argv[++i]);
            }
            else
            {
                filename = argv[i];
            }
            ++i;
        }
    }
    else
    {
        usage(argv[0]);
        ::system("PAUSE");
        exit(-1);
    }

    initialize_log(LOGNAME, log);

    Category * logger = &Category::getInstance(LOGNAME);

    //if (!data_access::UniDataConnection::openConnection(L"localhost", 3306, L"base", L"teddy", L"teddy"))
    if (!data_access::UniDataConnection::openConnection(UCS(""), 0, encode<ansi,ucs>(base), UCS(""), UCS("")))
    {
        cout << "cannot open DB connection." << endl;
        ::system("PAUSE");
        exit(-1);
    }

    ucs::string_t buf;
    shared_ptr< Term<ucs> > a;
    if (readFile(filename, buf) &&
        Program<ucs>::parse(buf, a))
    {
        Context<ucs> c;
        a->interpret(c);
        for (Context<ucs>::iterator_t i = c.begin(); i != c.end(); ++i)
        {
            logger->debugStream() << "[" << encode<ucs,ansi>(c.getIdentifier(i)) << "] = "
            << encode<ucs,ansi>(c.getObject(i)->toString());
        }
        logger->noticeStream() << "script OK\n";
    }
    else
    {
        logger->noticeStream() << "script NOK\n";
    }

    ::system("PAUSE");
    return EXIT_SUCCESS;
}


text:
// Wheels - various C++ utilities
//
// Written in 2012 by Martinho Fernandes <martinho.fernandes@gmail.com>
//
// To the extent possible under law, the author(s) have dedicated all copyright and related
// and neighboring rights to this software to the public domain worldwide. This software is
// distributed without any warranty.
//
// You should have received a copy of the CC0 Public Domain Dedication along with this software.
// If not, see <http://creativecommons.org/publicdomain/zero/1.0/>.

// Tests for <wheels/overload.h++>

#include <wheels/overload.h++>

#include <wheels/unit_test.h++>

#include <wheels/concept.h++>

#include <boost/variant/variant.hpp>
#include <boost/variant/apply_visitor.hpp>

#include <string>

int fun0(int n) {
    return n * 2;
}

int fun1(double n) {
    return static_cast<int>(n);
}

int fun2(std::string const& n) {
    return n.size();
}

int overloaded(int n) {
    return n * 2;
}
int overloaded(double n) {
    return static_cast<int>(n);
}
int overloaded(std::string const& n) {
    return n.size();
}

TEST_CASE("overload", "overloaded function wrapper") {
    SECTION("move", "move semantics") {
        struct funobj {
            int operator()(int n) const { return p? *p + n : -1; }
            std::unique_ptr<int> p;
        } f { std::unique_ptr<int>(new int(17)) };
        auto overload = wheels::make_overload(std::move(f));
        auto moved = std::move(overload);
        CONCEPT_CHECK(decltype(overload),
                      MoveSemantics,
                      with_signature<int(int)>::Callable
                     );
        CHECK(overload(23) == -1);
        CHECK(moved(23) == 17+23);
    }
    SECTION("ref", "ref wrapper") {
        struct foo {
            foo() = default;
            foo(foo const&) = delete;

            const foo* operator()() const { return this; }
        } no_copy;
        auto overload = wheels::make_overload(std::ref(no_copy));
        CHECK(overload() == &no_copy);
    }
}

TEST_CASE("overload/single", "overload with a single function") {
    SECTION("funobj", "function object") {
        struct funobj {
            int operator()(int n) const { return 17 + n; }
        } f;
        auto overload = wheels::make_overload(f);
        CONCEPT_CHECK(decltype(overload),
                      DefaultConstructible,
                      CopySemantics,
                      with_signature<void(int)>::Callable,
                      with_signature<int(int)>::Callable
                     );
        CHECK(overload(23) == 17+23);
    }
    SECTION("funptr", "function pointer") {
        auto overload = wheels::make_overload(fun0);
        CONCEPT_CHECK(decltype(overload),
                      CopySemantics,
                      with_signature<void(int)>::Callable,
                      with_signature<int(int)>::Callable
                     );
        CHECK(overload(42) == 42*2);
    }
    SECTION("memfun", "member function pointer") {
        struct foo {
            int f(int n) { return n + delta; }
            int delta;
        } x { 3 };
        auto overload = wheels::make_overload(&foo::f);
        CONCEPT_CHECK(decltype(overload),
                      CopySemantics,
                      with_signature<void(foo&,int)>::Callable,
                      with_signature<int(foo&,int)>::Callable
                     );
        CHECK(overload(x, 23) == 23+x.delta);
    }
}

TEST_CASE("overload/multiple", "overload with multiple functions") {
    SECTION("funobj", "function object") {
        struct funobj0 {
            int operator()(int n) const { return 17 + n; }
            double operator()(double d) const { return 23 + d; }
        } f0;
        struct funobj1 {
            int operator()(std::string const& s) const { return 42 + s.size(); }
        } f1;
        auto overload = wheels::make_overload(f0, f1);
        CONCEPT_CHECK(decltype(overload),
                      DefaultConstructible,
                      CopySemantics,
                      with_signature<int(int)>::Callable,
                      with_signature<int(double)>::Callable,
                      with_signature<double(double)>::Callable,
                      with_signature<int(std::string const&)>::Callable
                     );
        CHECK(overload(5) == 17+5);
        CHECK(overload(0.5) == 23 + 0.5);
        CHECK(overload("foo") == 42+3);
    }
    SECTION("funptr", "function pointer") {
        auto overload = wheels::make_overload(fun0, fun1, fun2);
        CONCEPT_CHECK(decltype(overload),
                      CopySemantics,
                      with_signature<int(int)>::Callable,
                      with_signature<int(double)>::Callable,
                      with_signature<double(double)>::Callable,
                      with_signature<int(std::string const&)>::Callable
                     );
        CHECK(overload(42) == 42*2);
        CHECK(overload(23.5) == 23);
        CHECK(overload("foo") == 3);
    }
    SECTION("memfun", "member function pointer") {
        struct foo {
            int f(int n) { return n + delta; }
            double g(double d) { return d + delta*0.1; }
            int h(std::string const& s) { return s.size() + delta; }
            int delta;
        } x { 3 };
        auto overload = wheels::make_overload(&foo::f, &foo::g, &foo::h);
        CONCEPT_CHECK(decltype(overload),
                      CopySemantics,
                      with_signature<void(foo&,int)>::Callable,
                      with_signature<int(foo&,int)>::Callable
                     );
        CHECK(overload(x, 23) == 23+x.delta);
        CHECK(overload(x, 17.0) == 17.0+x.delta*0.1);
        CHECK(overload(x, "foo") == 3+x.delta);
    }
    SECTION("mixed", "mixed callable kinds") {
        struct funobj0 {
            int operator()(double n) const { return 17.6 + n; }
            int operator()(std::string const& s) const { return 42 + s.size(); }
        } f0;
        struct foo {
            int f(std::string const& s) { return s.size() + delta; }
            int g(std::u16string const& s) { return s.size() + delta*2; }
            int delta;
        } x { 3 };
        auto overload = wheels::make_overload(fun0, f0, &foo::f, &foo::g);
        CONCEPT_CHECK(decltype(overload),
                      CopySemantics,
                      with_signature<int(int)>::Callable,
                      with_signature<int(double)>::Callable,
                      with_signature<int(std::string const&)>::Callable,
                      with_signature<int(foo&, std::string const&)>::Callable,
                      with_signature<int(foo&, std::u16string const&)>::Callable
                     );
        CHECK(overload(42) == 42*2);
        CHECK(overload(0.5) == 18);
        CHECK(overload("foo") == 42+3);
        CHECK(overload(x, "foo") == 3+x.delta);
        CHECK(overload(x, u"foo") == 3+x.delta*2);
    }
#if !WHEELS_HAS_ISSUE(BROKEN_OVERLOAD_MACRO)
    SECTION("overloaded", "overloaded names") {
        SECTION("variadic", "all types listed") {
            auto overload = wheels::make_overload<int(int), int(double), int(std::string const&)>(WHEELS_OVERLOAD(overloaded));
            CONCEPT_CHECK(decltype(overload),
                          CopySemantics,
                          with_signature<int(int)>::Callable,
                          with_signature<int(double)>::Callable,
                          with_signature<int(std::string const&)>::Callable
                         );
            CHECK(overload(42) == 42*2);
            CHECK(overload(17.5) == 17);
            CHECK(overload("foobar") == 6);
        }
    }
#endif
}

TEST_CASE("overload/visitor", "make_visitor tests") {
    int called = 0;
    auto fun1 = [&called] (int) { called = 1; };
    auto fun2 = [&called] (std::string const&) { called = 2; };

    auto visitor = wheels::make_visitor(fun1, fun2);
    CONCEPT_CHECK(decltype(visitor),
                  CopyConstructible,
                  with_signature<void(int)>::Callable,
                  with_signature<void(std::string const&)>::Callable
                 );

    using test_variant = boost::variant<int, std::string>;
    test_variant var { 42 };

    CHECK(called == 0);
    boost::apply_visitor(visitor, var);
    CHECK(called == 1);

    called = 0;

    var = "foo";
    boost::apply_visitor(visitor, var);
    CHECK(called == 2);
}

max_stars_repo_name: picanumber/named-operator

max_issues_repo_name: picanumber/named-operator

max_forks_repo_name: picanumber/named-operator