// DO NOT EVER INCLUDE THIS HEADER FILE YOURSELF

#ifndef __lcm_cpp_impl_ok__
#error "Don't include this file"
#endif

// =============== implementation ===============

int
Subscription::setQueueCapacity(int num_messages)
{
    return lcm_subscription_set_queue_capacity(c_subs, num_messages);
}

template <class MessageType, class ContextClass>
class LCMTypedSubscription : public Subscription {
    friend class LCM;
    private:
        ContextClass context;
        void (*handler)(const ReceiveBuffer *rbuf, const std::string& channel,
                const MessageType*msg, ContextClass context);
        static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
                void *user_data)
        {
            typedef LCMTypedSubscription<MessageType,ContextClass> SubsClass;
            SubsClass *subs = static_cast<SubsClass *> (user_data);
            MessageType msg;
            int status = msg.decode(rbuf->data, 0, rbuf->data_size);
            if (status < 0) {
                fprintf (stderr, "error %d decoding %s!!!\n", status,
                        MessageType::getTypeName());
                return;
            }
            const ReceiveBuffer rb = {
                rbuf->data,
                rbuf->data_size,
                rbuf->recv_utime
            };
            subs->handler(&rb, channel, &msg, subs->context);
        }
};

template <class ContextClass>
class LCMUntypedSubscription : public Subscription {
    friend class LCM;
    private:
        ContextClass context;
        void (*handler)(const ReceiveBuffer *rbuf, const std::string& channel,
                ContextClass context);
        static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
                void *user_data)
        {
            typedef LCMUntypedSubscription<ContextClass> SubsClass;
            SubsClass *subs = static_cast<SubsClass *> (user_data);
            const ReceiveBuffer rb = {
                rbuf->data,
                rbuf->data_size,
                rbuf->recv_utime
            };
            subs->handler(&rb, channel, subs->context);
        }
};

template <class MessageType, class MessageHandlerClass>
class LCMMHSubscription : public Subscription {
    friend class LCM;
    private:
        MessageHandlerClass* handler;
        void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer* rbuf, const std::string& channel, const MessageType* msg);
        static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel,
                void *user_data)
        {
            LCMMHSubscription<MessageType,MessageHandlerClass> *subs =
                static_cast<LCMMHSubscription<MessageType,MessageHandlerClass> *>(user_data);
            MessageType msg;
            int status = msg.decode(rbuf->data, 0, rbuf->data_size);
            if (status < 0) {
                fprintf (stderr, "error %d decoding %s!!!\n", status,
                        MessageType::getTypeName());
                return;
            }
            const ReceiveBuffer rb = {
                rbuf->data,
                rbuf->data_size,
                rbuf->recv_utime
            };
            std::string chan_str(channel);
            (subs->handler->*subs->handlerMethod)(&rb, chan_str, &msg);
        }
};

template<class MessageHandlerClass>
class LCMMHUntypedSubscription : public Subscription {
    friend class LCM;
    private:
        MessageHandlerClass* handler;
        void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer* rbuf, const std::string& channel);
        static void cb_func(const lcm_recv_buf_t *rbuf, const char *channel, void *user_data)
        {
            LCMMHUntypedSubscription<MessageHandlerClass> *subs =
                static_cast<LCMMHUntypedSubscription<MessageHandlerClass> *>(user_data);
            const ReceiveBuffer rb = {
                rbuf->data,
                rbuf->data_size,
                rbuf->recv_utime
            };
            std::string chan_str(channel);
            (subs->handler->*subs->handlerMethod)(&rb, chan_str);
        }
};

inline
LCM::LCM(std::string lcm_url):
owns_lcm(true)
{
    this->lcm = lcm_create(lcm_url.c_str());
}

inline
LCM::LCM(lcm_t * lcm_in):
owns_lcm(false)
{
    this->lcm = lcm_in;
}

inline bool
LCM::good() const
{
    return this->lcm != NULL;
}

inline
LCM::~LCM() {
    for(int i=0, n=subscriptions.size(); i<n; i++) {
        delete subscriptions[i];
    }
    if(this->lcm && this->owns_lcm) {
        lcm_destroy(this->lcm);
    }
}

inline int
LCM::publish(const std::string& channel, const void *data, unsigned int datalen) {
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to publish()\n");
        return -1;
    }
    return lcm_publish(this->lcm, channel.c_str(), data, datalen);
}

template<class MessageType>
inline int
LCM::publish(const std::string& channel, const MessageType *msg) {
    unsigned int datalen = msg->getEncodedSize();
    uint8_t *buf = new uint8_t[datalen];
    msg->encode(buf, 0, datalen);
    int status = this->publish(channel, buf, datalen);
    delete[] buf;
    return status;
}

inline int
LCM::unsubscribe(Subscription *subscription) {
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to unsubscribe()\n");
        return -1;
    }
    std::vector<Subscription*>::iterator iter;
    std::vector<Subscription*>::iterator eiter = subscriptions.end();
    for(iter=subscriptions.begin(); iter!= eiter; ++iter) {
        if(*iter == subscription) {
            int status = lcm_unsubscribe(lcm, subscription->c_subs);
            subscriptions.erase(iter);
            delete subscription;
            return status;
        }
    }
    return -1;
}

inline int
LCM::getFileno() {
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to fileno()\n");
        return -1;
    }
    return lcm_get_fileno(this->lcm);
}

inline int
LCM::handle() {
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to handle()\n");
        return -1;
    }
    return lcm_handle(this->lcm);
}

inline int
LCM::handleTimeout(int timeout_millis) {
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to handle()\n");
        return -1;
    }
    return lcm_handle_timeout(this->lcm, timeout_millis);
}

template <class MessageType, class MessageHandlerClass>
Subscription*
LCM::subscribe(const std::string& channel,
    void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer* rbuf, const std::string& channel, const MessageType* msg),
    MessageHandlerClass* handler)
{
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to subscribe()\n");
        return NULL;
    }
    LCMMHSubscription<MessageType, MessageHandlerClass> *subs =
        new LCMMHSubscription<MessageType, MessageHandlerClass>();
    subs->handler = handler;
    subs->handlerMethod = handlerMethod;
    subs->c_subs = lcm_subscribe(this->lcm, channel.c_str(),
            LCMMHSubscription<MessageType, MessageHandlerClass>::cb_func, subs);
    subscriptions.push_back(subs);
    return subs;
}

template <class MessageHandlerClass>
Subscription*
LCM::subscribe(const std::string& channel,
    void (MessageHandlerClass::*handlerMethod)(const ReceiveBuffer* rbuf, const std::string& channel),
    MessageHandlerClass* handler)
{
    if(!this->lcm) {
        fprintf(stderr,
            "LCM instance not initialized.  Ignoring call to subscribe()\n");
        return NULL;
    }
    LCMMHUntypedSubscription<MessageHandlerClass> *subs =
        new LCMMHUntypedSubscription<MessageHandlerClass>();
    subs->handler = handler;
    subs->handlerMethod = handlerMethod;
    subs->c_subs = lcm_subscribe(this->lcm, channel.c_str(),
            LCMMHUntypedSubscription<MessageHandlerClass>::cb_func, subs);
    subscriptions.push_back(subs);
    return subs;
}

template <class MessageType, class ContextClass>
Subscription*
LCM::subscribeFunction(const std::string& channel,
        void (*handler)(const ReceiveBuffer *rbuf,
            const std::string& channel,
            const MessageType *msg, ContextClass context),
        ContextClass context) {
    if(!this->lcm) {
        fprintf(stderr, "LCM instance not initialized.  Ignoring call to subscribeFunction()\n");
        return NULL;
    }
    typedef LCMTypedSubscription<MessageType, ContextClass> SubsClass;
    SubsClass *sub = new SubsClass();
    sub->c_subs = lcm_subscribe(lcm, channel.c_str(), SubsClass::cb_func, sub);
    sub->handler = handler;
    sub->context = context;
    subscriptions.push_back(sub);
    return sub;
}

template <class ContextClass>
Subscription*
LCM::subscribeFunction(const std::string& channel,
        void (*handler)(const ReceiveBuffer *rbuf,
                       const std::string& channel,
                       ContextClass context),
        ContextClass context) {
    if(!this->lcm) {
        fprintf(stderr, "LCM instance not initialized.  Ignoring call to subscribeFunction()\n");
        return NULL;
    }
    typedef LCMUntypedSubscription<ContextClass> SubsClass;
    SubsClass *sub = new SubsClass();
    sub->c_subs = lcm_subscribe(lcm, channel.c_str(), SubsClass::cb_func, sub);
    sub->handler = handler;
    sub->context = context;
    subscriptions.push_back(sub);
    return sub;
}


lcm_t*
LCM::getUnderlyingLCM()
{
    return this->lcm;
}

LogFile::LogFile(const std::string & path, const std::string & mode) :
  eventlog(lcm_eventlog_create(path.c_str(), mode.c_str())),
  last_event(NULL)
{
}

LogFile::~LogFile()
{
    if(eventlog)
        lcm_eventlog_destroy(eventlog);
    eventlog = NULL;
    if(last_event)
        lcm_eventlog_free_event(last_event);
    last_event = NULL;
}

bool
LogFile::good() const
{
    return eventlog != NULL;
}

const LogEvent*
LogFile::readNextEvent()
{
    lcm_eventlog_event_t* evt = lcm_eventlog_read_next_event(eventlog);
    if(last_event)
        lcm_eventlog_free_event(last_event);
    last_event = evt;
    if(!evt)
        return NULL;
    curEvent.eventnum = evt->eventnum;
    curEvent.timestamp = evt->timestamp;
    curEvent.channel.assign(evt->channel, evt->channellen);
    curEvent.datalen = evt->datalen;
    curEvent.data = evt->data;
    return &curEvent;
}

int
LogFile::seekToTimestamp(int64_t timestamp)
{
    return lcm_eventlog_seek_to_timestamp(eventlog, timestamp);
}

int
LogFile::writeEvent(LogEvent* event)
{
    lcm_eventlog_event_t evt;
    evt.eventnum = event->eventnum;
    evt.timestamp = event->timestamp;
    evt.channellen = event->channel.size();
    evt.datalen = event->datalen;
    evt.channel = (char*) event->channel.c_str();
    evt.data = event->data;
    return lcm_eventlog_write_event(eventlog, &evt);
}

FILE*
LogFile::getFilePtr()
{
    return eventlog->f;
}