#include <iostream>
#include <vector>
#include <functional>
#include <cassert>
#include <set>
#include <string>
#include <map>
#include <typeindex>
#include <memory>

namespace s28_cb {

struct CallbackVector {
  virtual ~CallbackVector() = default;
  virtual void call(void *raw) = 0;
  virtual void unregister(int id) = 0;
};

template<typename Message>
struct CallbackVectorImpl : CallbackVector {
  void call(void *raw) override {
    auto *msg = static_cast<Message *>(raw);
    for (auto &h: callbacks_) {
      h.fn(msg);
    }
  }

  void unregister(int id) override {
    callbacks_.erase(
      std::remove_if(callbacks_.begin(), callbacks_.end(),
                     [id](const Holder &h) { return h.id == id; }),
      callbacks_.end());
  }

  int insert(std::function<void(Message *)> &&fn, int priority) {
    Holder h;
    h.id = next_id_++;
    h.fn = std::move(fn);
    h.priority = priority;
    callbacks_.push_back(h);

    std::stable_sort(callbacks_.begin(), callbacks_.end(),
    [](const Holder &a, const Holder &b) {
        return a.priority < b.priority;
    });

    return h.id;
  }

private:
  struct Holder {
    int id;
    std::function<void(Message *)> fn;
    int priority = 0;
  };

  std::vector<Holder> callbacks_;
  int next_id_ = 1;
};


using CallbackId = std::pair<int, size_t>;

struct CallbackRegister {
  template<typename Message>
  void call(Message *msg) {
    size_t id = get_type_id<Message>();
    if (id >= message_callbacks_.size()) {
      return;
    }

    CallbackVector * cb = message_callbacks_[id].get();
    if (!cb) {
      return;
    }
    cb->call(msg);
  }

  template<typename Message>
  CallbackId reg(std::function<void(Message*)> fn, int priority = 0) {
    size_t id = get_type_id<Message>();
    if (message_callbacks_.size() <= id) {
      message_callbacks_.resize(id + 1);
    }

    if (!message_callbacks_[id]) {
      message_callbacks_[id] = std::make_unique<CallbackVectorImpl<Message>>();
    }

    auto* impl = static_cast<CallbackVectorImpl<Message>*>(message_callbacks_[id].get());

    CallbackId rv{};
    rv.second = id;    
    rv.first = impl->insert(std::move(fn), priority);
    return rv;
  }

  template<typename Class, typename Message>
  CallbackId reg(Class* instance, void (Class::*method)(Message*), int priority = 0) {
      auto fn = [instance, method](Message* msg) {
          (instance->*method)(msg);
      };
      return reg<Message>(std::move(fn), priority);
  }

  void unregister(CallbackId cbid) {
    if (cbid.second >= message_callbacks_.size())
      return;
    if (!message_callbacks_[cbid.second])
      return;
    message_callbacks_[cbid.second]->unregister(cbid.first);
  };

  template<typename Fn>
  void reg(Fn&& fn) {
      using ArgType = typename get_message_type<Fn>::type;
      reg<ArgType>(std::function<void(ArgType*)>(std::forward<Fn>(fn)));
  }

private:
  // SFINAE to extract the lambda parameter type
  template<typename Fn>
  struct get_message_type;

  template<typename Ret, typename C, typename Arg>
  struct get_message_type<Ret(C::*)(Arg*) const> {
      using type = Arg;
  };

  template<typename Ret, typename Arg>
  struct get_message_type<Ret(*)(Arg*)> {
      using type = Arg;
  };

  template<typename Fn>
  struct get_message_type : get_message_type<decltype(&Fn::operator())> {};


  template<typename T>
  size_t get_type_id() {
    static const size_t id = next_id_++;
    return id;
  }

  static inline size_t next_id_ = 0;
  std::vector<std::unique_ptr<CallbackVector>>  message_callbacks_;
};

} // namespace s28_cb

struct Msg1 {};
struct Msg2 {};

template<typename Class>
struct Mod {
  virtual ~Mod() {
    if (!callback_register_)
      return;
    for (auto &cbid: registered_callbacks_) {
      callback_register_->unregister(cbid);
    }
  }

  template<typename Message>
  void reg(void (Class::*method)(Message*), int priority = 0) {
    assert(callback_register_);
    auto res = callback_register_->reg(static_cast<Class*>(this), method, priority);
    registered_callbacks_.push_back(res);
  }
  void set_callback_register(s28_cb::CallbackRegister* cb) {
    assert(!callback_register_);
    assert(cb);
    callback_register_ = cb;
  }
private:
  s28_cb::CallbackRegister *callback_register_ = nullptr;
  std::vector<s28_cb::CallbackId> registered_callbacks_;
};

struct Test : Mod<Test> {
  void initialize() {
    reg(&Test::message_handler1);
    reg(&Test::message_handler2, -1);
    reg(&Test::message_handler3, -1);
  }

  void message_handler1(Msg1 *) {
       std::cout << "msg1 callback1" << std::endl;
  }

  void message_handler2(Msg1 *) {
       std::cout << "msg1 callback2" << std::endl;
  }

  void message_handler3(Msg2 *) {
       std::cout << "msg2 callback1" << std::endl;
  }
};

int main() {
  s28_cb::CallbackRegister callback_register;
  Test test;
  test.set_callback_register(&callback_register);
  test.initialize();

  Msg2 msg;
  callback_register.call(&msg);
}