<commit_before>/*
 * Copyright (C) 2010-2012 Jeremy Lainé
 * Contact: http://code.google.com/p/qdjango/
 *
 * This file is part of the QDjango Library.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 */

#include "QDjango.h"
#include "QDjangoModel.h"
#include "QDjangoQuerySet.h"
#include "QDjangoWhere.h"
#include "util.h"

class Author : public QDjangoModel
{
    Q_OBJECT
    Q_PROPERTY(QString name READ name WRITE setName)

public:
    Author(QObject *parent = 0);

    QString name() const;
    void setName(const QString &name);

private:
    QString m_name;
};

class Book : public QDjangoModel
{
    Q_OBJECT
    Q_PROPERTY(Author* author READ author WRITE setAuthor)
    Q_PROPERTY(QString title READ title WRITE setTitle)

public:
    Book(QObject *parent = 0);

    Author *author() const;
    void setAuthor(Author *author);

    QString title() const;
    void setTitle(const QString &title);

    // expose foreign key methods so they can be tested
    QObject *foreignKey(const char *name) const
    { return QDjangoModel::foreignKey(name); }
    void setForeignKey(const char *name, QObject *value)
    { QDjangoModel::setForeignKey(name, value); }
private:
    QString m_title;
};

/** Test QDjangoModel class.
 */
class tst_QDjangoModel : public QObject
{
    Q_OBJECT

private slots:
    void initTestCase();
    void init();
    void foreignKey();
    void setForeignKey();
    void filterRelated();
    void selectRelated();
    void cleanup();
    void cleanupTestCase();
};

Author::Author(QObject *parent)
    : QDjangoModel(parent)
{
}

QString Author::name() const
{
    return m_name;
}

void Author::setName(const QString &name)
{
    m_name = name;
}

Book::Book(QObject *parent)
    : QDjangoModel(parent)
{
    setForeignKey("author", new Author(this));
}

Author* Book::author() const
{
    return qobject_cast<Author*>(foreignKey("author"));
}

void Book::setAuthor(Author *author)
{
    setForeignKey("author", author);
}

QString Book::title() const
{
    return m_title;
}

void Book::setTitle(const QString &title)
{
    m_title = title;
}

/** Create database tables before running tests.
 */
void tst_QDjangoModel::initTestCase()
{
    QVERIFY(initialiseDatabase());
    QCOMPARE(QDjango::registerModel<Author>().createTable(), true);
    QCOMPARE(QDjango::registerModel<Book>().createTable(), true);
}

/** Load fixtures.
 */
void tst_QDjangoModel::init()
{
    Author author1;
    author1.setName("First author");
    QCOMPARE(author1.save(), true);

    Author author2;
    author2.setName("Second author");
    QCOMPARE(author2.save(), true);

    Book book;
    book.setAuthor(&author1);
    book.setTitle("Some book");
    QCOMPARE(book.save(), true);

    Book book2;
    book2.setAuthor(&author2);
    book2.setTitle("Other book");
    QCOMPARE(book2.save(), true);
}

void tst_QDjangoModel::foreignKey()
{
    QTest::ignoreMessage(QtWarningMsg, "QDjangoMetaModel cannot get foreign model for invalid key 'bad'");
    Book book;
    QVERIFY(!book.foreignKey("bad"));
    QVERIFY(book.foreignKey("author"));
}

void tst_QDjangoModel::setForeignKey()
{
    QTest::ignoreMessage(QtWarningMsg, "QDjangoMetaModel cannot set foreign model for invalid key 'bad'");
    Book book;
    book.setForeignKey("bad", 0);
    book.setForeignKey("author", 0);
}

/** Perform filtering on foreign keys.
 */
void tst_QDjangoModel::filterRelated()
{
    QDjangoQuerySet<Book> books;

    QDjangoQuerySet<Book> qs = books.filter(
        QDjangoWhere("author__name", QDjangoWhere::Equals, "First author"));
    CHECKWHERE(qs.where(), QLatin1String("T0.\"name\" = ?"), QVariantList() << "First author");
    QCOMPARE(qs.count(), 1);
    QCOMPARE(qs.size(), 1);

    Book *book = qs.at(0);
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    delete book;
}

/** Test eager loading of foreign keys.
 */
void tst_QDjangoModel::selectRelated()
{
    // without eager loading
    QDjangoQuerySet<Book> qs;
    Book *book = qs.get(QDjangoWhere("title", QDjangoWhere::Equals, "Some book"));
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    QVERIFY(book->author() != 0);
    QCOMPARE(book->author()->name(), QLatin1String("First author"));
    delete book;

    // with eager loading
    book = qs.selectRelated().get(QDjangoWhere("title", QDjangoWhere::Equals, "Some book"));
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    QVERIFY(book->author() != 0);
    QCOMPARE(book->author()->name(), QLatin1String("First author"));
    delete book;
}

/** Clear database tables after each test.
 */
void tst_QDjangoModel::cleanup()
{
    QCOMPARE(QDjangoQuerySet<Book>().remove(), true);
    QCOMPARE(QDjangoQuerySet<Author>().remove(), true);
}

/** Drop database tables after running tests.
 */
void tst_QDjangoModel::cleanupTestCase()
{
    QCOMPARE(QDjango::registerModel<Book>().dropTable(), true);
    QCOMPARE(QDjango::registerModel<Author>().dropTable(), true);
}

QTEST_MAIN(tst_QDjangoModel)
#include "tst_qdjangomodel.moc"
<commit_msg>refactor tests<commit_after>/*
 * Copyright (C) 2010-2012 Jeremy Lainé
 * Contact: http://code.google.com/p/qdjango/
 *
 * This file is part of the QDjango Library.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 */

#include "QDjango.h"
#include "QDjangoModel.h"
#include "QDjangoQuerySet.h"
#include "QDjangoWhere.h"
#include "util.h"

class TestModel : public QDjangoModel
{
public:
    TestModel(QObject *parent = 0) : QDjangoModel(parent) {}

    // expose foreign key methods so they can be tested
    QObject *foreignKey(const char *name) const
    { return QDjangoModel::foreignKey(name); }
    void setForeignKey(const char *name, QObject *value)
    { QDjangoModel::setForeignKey(name, value); }
};

class Author : public TestModel
{
    Q_OBJECT
    Q_PROPERTY(QString name READ name WRITE setName)

public:
    Author(QObject *parent = 0) : TestModel(parent) {}

    QString name() const { return m_name; }
    void setName(const QString &name) { m_name = name; }

private:
    QString m_name;
};

class Book : public TestModel
{
    Q_OBJECT
    Q_PROPERTY(Author* author READ author WRITE setAuthor)
    Q_PROPERTY(QString title READ title WRITE setTitle)

public:
    Book(QObject *parent = 0)
        : TestModel(parent)
    {
        setForeignKey("author", new Author(this));
    }

    Author *author() const { return qobject_cast<Author*>(foreignKey("author")); }
    void setAuthor(Author *author) { setForeignKey("author", author); }

    QString title() const { return m_title; }
    void setTitle(const QString &title) { m_title = title; }

private:
    QString m_title;
};

/** Test QDjangoModel class.
 */
class tst_QDjangoModel : public QObject
{
    Q_OBJECT

private slots:
    void initTestCase();
    void init();
    void foreignKey();
    void setForeignKey();
    void filterRelated();
    void selectRelated();
    void cleanup();
    void cleanupTestCase();
};

/** Create database tables before running tests.
 */
void tst_QDjangoModel::initTestCase()
{
    QVERIFY(initialiseDatabase());
    QCOMPARE(QDjango::registerModel<Author>().createTable(), true);
    QCOMPARE(QDjango::registerModel<Book>().createTable(), true);
}

/** Load fixtures.
 */
void tst_QDjangoModel::init()
{
    Author author1;
    author1.setName("First author");
    QCOMPARE(author1.save(), true);

    Author author2;
    author2.setName("Second author");
    QCOMPARE(author2.save(), true);

    Book book;
    book.setAuthor(&author1);
    book.setTitle("Some book");
    QCOMPARE(book.save(), true);

    Book book2;
    book2.setAuthor(&author2);
    book2.setTitle("Other book");
    QCOMPARE(book2.save(), true);
}

void tst_QDjangoModel::foreignKey()
{
    QTest::ignoreMessage(QtWarningMsg, "QDjangoMetaModel cannot get foreign model for invalid key 'bad'");
    Book book;
    QVERIFY(!book.foreignKey("bad"));
    QVERIFY(book.foreignKey("author"));
}

void tst_QDjangoModel::setForeignKey()
{
    QTest::ignoreMessage(QtWarningMsg, "QDjangoMetaModel cannot set foreign model for invalid key 'bad'");
    Book book;
    book.setForeignKey("bad", 0);
    book.setForeignKey("author", 0);
}

/** Perform filtering on foreign keys.
 */
void tst_QDjangoModel::filterRelated()
{
    QDjangoQuerySet<Book> books;

    QDjangoQuerySet<Book> qs = books.filter(
        QDjangoWhere("author__name", QDjangoWhere::Equals, "First author"));
    CHECKWHERE(qs.where(), QLatin1String("T0.\"name\" = ?"), QVariantList() << "First author");
    QCOMPARE(qs.count(), 1);
    QCOMPARE(qs.size(), 1);

    Book *book = qs.at(0);
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    delete book;
}

/** Test eager loading of foreign keys.
 */
void tst_QDjangoModel::selectRelated()
{
    // without eager loading
    QDjangoQuerySet<Book> qs;
    Book *book = qs.get(QDjangoWhere("title", QDjangoWhere::Equals, "Some book"));
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    QVERIFY(book->author() != 0);
    QCOMPARE(book->author()->name(), QLatin1String("First author"));
    delete book;

    // with eager loading
    book = qs.selectRelated().get(QDjangoWhere("title", QDjangoWhere::Equals, "Some book"));
    QVERIFY(book != 0);
    QCOMPARE(book->title(), QLatin1String("Some book"));
    QVERIFY(book->author() != 0);
    QCOMPARE(book->author()->name(), QLatin1String("First author"));
    delete book;
}

/** Clear database tables after each test.
 */
void tst_QDjangoModel::cleanup()
{
    QCOMPARE(QDjangoQuerySet<Book>().remove(), true);
    QCOMPARE(QDjangoQuerySet<Author>().remove(), true);
}

/** Drop database tables after running tests.
 */
void tst_QDjangoModel::cleanupTestCase()
{
    QCOMPARE(QDjango::registerModel<Book>().dropTable(), true);
    QCOMPARE(QDjango::registerModel<Author>().dropTable(), true);
}

QTEST_MAIN(tst_QDjangoModel)
#include "tst_qdjangomodel.moc"
<|endoftext|>


<commit_before>#pragma once

#include <iostream>
#include <cstdint>
#include <string>
#include <deque>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <typeinfo>
#include <cxxabi.h>
#include <sys/time.h>
#include <stdarg.h>

#include <zcm/zcm-cpp.hpp>
#include <zcm/util/Filter.hpp>

static bool __ZCM_DEBUG_ENABLED__ = (NULL != getenv("ZCM_DEBUG"));
#define ZCM_DEBUG(...) \
    do { \
        if (__ZCM_DEBUG_ENABLED__) \
            __ZCM_PRINT_OBFUSCATE__(std::cout, "ZCM-DEBUG: ", __VA_ARGS__, '\n'); \
    } while(0)

namespace zcm {

template <typename T>
class MessageTracker
{
  public:
    // You must free the memory passed into this callback
    typedef void (*callback)(T* msg, uint64_t utime, void* usr);
    static const bool NONBLOCKING = false;
    static const bool BLOCKING = true;

  protected:
    virtual uint64_t getMsgUtime(const T* msg)
    {
        return hasUtime<T>::utime(msg);
    }

    // The returned value must be "new" in all cases
    virtual T* interpolate(uint64_t utimeTarget,
                           const T* A, uint64_t utimeA,
                           const T* B, uint64_t utimeB)
    {
        return utimeTarget - utimeA < utimeB - utimeTarget ? new T(*A) : new T(*B);
    }

  private:
    zcm::ZCM* zcmLocal = nullptr;

    uint64_t maxTimeErr_us;

    std::atomic_bool done {false};

    std::deque<T*> buf;
    size_t bufMax;
    std::mutex bufLock;
    std::condition_variable newMsgCond;
    zcm::Subscription *s = nullptr;

    std::mutex callbackLock;
    std::condition_variable callbackCv;
    T* callbackMsg = nullptr;
    std::thread *thr = nullptr;
    callback onMsg;
    void* usr;

    Filter hzFilter;
    Filter jitterFilter;

    void callbackThreadFunc()
    {
        std::unique_lock<std::mutex> lk(callbackLock);
        while (!done) {
            callbackCv.wait(lk, [&](){ return callbackMsg || done; });
            if (done) return;
            onMsg(callbackMsg, getMsgUtime(callbackMsg), usr);
            // Intentionally not deleting callbackMsg as it is the
            // responsibility of the callback to delete the memory
            callbackMsg = nullptr;
        }
    }

    void handle(const zcm::ReceiveBuffer* rbuf, const std::string& chan, const T* _msg)
    {
        newMsg(_msg);
    }

    MessageTracker() {}

  public:
    MessageTracker(zcm::ZCM* zcmLocal, std::string channel,
                   double maxTimeErr = 0.25, size_t maxMsgs = 1,
                   callback onMsg = nullptr, void* usr = nullptr,
                   double freqEstConvergenceNumMsgs = 5)
        : zcmLocal(zcmLocal), maxTimeErr_us(maxTimeErr * 1e6), onMsg(onMsg), usr(usr),
              hzFilter(Filter::convergenceTimeToNatFreq(freqEstConvergenceNumMsgs, 0.8), 0.8),
          jitterFilter(Filter::convergenceTimeToNatFreq(freqEstConvergenceNumMsgs, 0.8), 0.8)
    {
        if (hasUtime<T>::present == true) {
            T tmp;
            std::string name = demangle(getType(tmp));
            ZCM_DEBUG("Message trackers using 'utime' field of zcmtype ", name);
        } else {
            T tmp;
            std::string name = demangle(getType(tmp));
            ZCM_DEBUG("Message trackers using local system receive utime for ",
                      "tracking zcmtype ", name);
        }

        bufMax = maxMsgs;

        if (onMsg != nullptr) thr = new std::thread(&MessageTracker<T>::callbackThreadFunc, this);
        if (zcmLocal && channel != "")
            s = zcmLocal->subscribe(channel, &MessageTracker<T>::handle, this);
    }

    virtual ~MessageTracker()
    {
        if (s) zcmLocal->unsubscribe(s);
        done = true;
        newMsgCond.notify_all();
        callbackCv.notify_all();
        if (thr) {
            thr->join();
            delete thr;
        }
        while (!buf.empty()) {
            T* tmp = buf.front();
            delete tmp;
            buf.pop_front();
        }
    }

    // You must free the memory returned here
    virtual T* get(bool blocking = NONBLOCKING)
    {
        T* ret = nullptr;

        {
            std::unique_lock<std::mutex> lk(bufLock);
            if (blocking == BLOCKING) {
                if (buf.empty())
                    newMsgCond.wait(lk, [&]{ return !buf.empty() || done; });
                if (done) return nullptr;
            }
            if (!buf.empty())
                ret = new T(*buf.back());
        }

        return ret;
    }

    // This may return nullptr even in the blocking case
    // TODO: Should consider how to allow the user to ask for an extrapolated
    //       message if bracketted messages arent available
    virtual T* get(uint64_t utime, bool blocking = NONBLOCKING)
    {
        T *m0 = nullptr, *m1 = nullptr; // two poses bracketing the desired utime
        uint64_t m0Utime = 0, m1Utime = UINT64_MAX;

        {
            std::unique_lock<std::mutex> lk(bufLock);

            if (blocking == BLOCKING) {
                // XXX This is technically not okay. "done" can't be an
                //     atomic as it can change after we've checked it in the
                //     wait. If it does change, and we go back to sleep,
                //     we'll never wake up. In other words, if done changes
                //     to true between the "if (done) return false;" and the
                //     end of the wait function, we would go back to sleep and
                //     never wake up again because this class would have already
                //     cleaned up
                newMsgCond.wait(lk, [&]{
                            if (done) return true;
                            if (buf.empty()) return false;
                            uint64_t recentUtime = getMsgUtime(buf.back());
                            if (recentUtime < utime) return false;
                            return true;
                        });
                if (done) return nullptr;
            }

            const T *_m0 = nullptr;
            const T *_m1 = nullptr;

            // The reason why we do a linear search is to support the ability
            // to skip around in logs. We want to support messages with
            // non-monitonically increasing utimes and this is the easiest way.
            // This can be made much faster if needed
            for (const T* m : buf) {
                uint64_t mUtime = getMsgUtime(m);

                if (mUtime <= utime && (_m0 == nullptr || mUtime > m0Utime)) {
                    _m0 = m;
                    m0Utime = mUtime;
                }

                if (mUtime >= utime && (_m1 == nullptr || mUtime < m1Utime)) {
                    _m1 = m;
                    m1Utime = mUtime;
                }
            }

            if (_m0 != nullptr) m0 = new T(*_m0);
            if (_m1 != nullptr) m1 = new T(*_m1);

        }

        if (m0 && utime - m0Utime > maxTimeErr_us) {
            delete m0;
            m0 = nullptr;
        }

        if (m1 && m1Utime - utime > maxTimeErr_us) {
            delete m1;
            m1 = nullptr;
        }

        if (m0 && m1) {
            if (m0Utime == m1Utime) {
                delete m1;
                return m0;
            }

            T* elt = interpolate(utime, m0, m0Utime, m1, m1Utime);

            delete m0;
            delete m1;

            return elt;
        }

        if (m0) return m0;
        if (m1) return m1;

        return nullptr;
    }

    /*
    virtual T* get(uint64_t utime, std::forward_iterator<T*> begin, std::forward_iterator<T*> end)

    virtual std::vector<T*> getRange(uint64_t utimeA, uint64_t utimeB,
                                     bool blocking = NONBLOCKING)
    {
        std::vector<T*> ret;
        return ret;
    }
    */

    virtual void newMsg(const T* _msg)
    {
        if (done) return;

        T* tmp = new T(*_msg);
        {
            std::unique_lock<std::mutex> lk(bufLock);

            uint64_t lastUtime = 0;
            if (!buf.empty()) lastUtime = getMsgUtime(buf.back());

            // Expire due to buffer being full
            if (buf.size() == bufMax){
                T* tmp = buf.front();
                delete tmp;
                buf.pop_front();
            }

            // Expire things that are too old
            while (!buf.empty()) {
                if (getMsgUtime(buf.front()) + maxTimeErr_us > getMsgUtime(_msg)) break;
                T* tmp = buf.front();
                delete tmp;
                buf.pop_front();
            }

            if (lastUtime != 0) {
                double obs = getMsgUtime(tmp) - lastUtime;
                hzFilter(obs, 1);
                double jitterObs = hzFilter[Filter::HIGH_PASS];
                jitterFilter(jitterObs * jitterObs, 1);
            }

            buf.push_back(tmp);
        }
        newMsgCond.notify_all();

        if (callbackLock.try_lock()) {
            if (callbackMsg) delete callbackMsg;
            callbackMsg = new T(*_msg);
            callbackLock.unlock();
            callbackCv.notify_all();
        }
    }

    virtual double getHz()
    {
        std::unique_lock<std::mutex> lk(bufLock);
        return 1e6 / hzFilter[Filter::LOW_PASS];
    }

    virtual double getJitterUs()
    {
        std::unique_lock<std::mutex> lk(bufLock);
        return sqrt(jitterFilter[Filter::LOW_PASS]);
    }

  private:
    // *****************************************************************************
    // Insanely hacky trick to determine at compile time if a zcmtype has a
    // field called "utime"
    // TODO: I think c++11 has some utilities to make this a bit easier (I have not
    //       looked into them much at all, but worth looking at <type_traits>
    //       and what you can do with things like std::true_type and the like)
    template <typename F> struct hasUtime {
        struct Fallback { void* utime; }; // introduce member name "utime"
        struct Derived : F, Fallback {};

        template <typename C, C> struct ChT;

        template <typename C>
        static uint64_t _utime(ChT<void* Fallback::*, &C::utime>*, const F* msg)
        {
            struct timeval tv;
            gettimeofday(&tv, NULL);
            return (uint64_t)tv.tv_sec * 1000000 + tv.tv_usec;
        };
        template <typename C>
        static uint64_t _utime(void* tmp, ...)
        {
            va_list args;
            va_start(args, tmp);
            const F* msg = va_arg(args, const F*);
            va_end(args);
            return msg->utime;
        }

        template<typename C> static char (&f(ChT<int Fallback::*, &C::x>*))[1];
        template<typename C> static char (&f(...))[2];

        static bool const present = sizeof(f<Derived>(0)) == 2;

        static uint64_t const utime(const F* msg)
        {
            return _utime<Derived>(0,msg);
        }
    };

    template<typename F>
    static inline std::string getType(const F t) { return typeid(t).name(); }

    static inline std::string demangle(std::string name)
    {
        int status = -4; // some arbitrary value to eliminate the compiler warning
        std::unique_ptr<char, void(*)(void*)> res {
            abi::__cxa_demangle(name.c_str(), NULL, NULL, &status),
            std::free
        };
        return (status==0) ? res.get() : name ;
    }

    static void __ZCM_PRINT_OBFUSCATE__(std::ostream& o) { }

    template<typename First, typename ...Rest>
    static void __ZCM_PRINT_OBFUSCATE__(std::ostream& o, First && first, Rest && ...rest)
    {
        o << std::forward<First>(first);
        __ZCM_PRINT_OBFUSCATE__(o, std::forward<Rest>(rest)...);
    }
    // *****************************************************************************

};

}

#undef ZCM_DEBUG
<commit_msg>Added range get and iterator get<commit_after>#pragma once

#include <iostream>
#include <cstdint>
#include <string>
#include <deque>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <typeinfo>
#include <cxxabi.h>
#include <sys/time.h>
#include <stdarg.h>

#include <zcm/zcm-cpp.hpp>
#include <zcm/util/Filter.hpp>

static bool __ZCM_DEBUG_ENABLED__ = (NULL != getenv("ZCM_DEBUG"));
#define ZCM_DEBUG(...) \
    do { \
        if (__ZCM_DEBUG_ENABLED__) \
            __ZCM_PRINT_OBFUSCATE__(std::cout, "ZCM-DEBUG: ", __VA_ARGS__, '\n'); \
    } while(0)

namespace zcm {

template <typename T>
class MessageTracker
{
  public:
    // You must free the memory passed into this callback
    typedef void (*callback)(T* msg, uint64_t utime, void* usr);
    static const bool NONBLOCKING = false;
    static const bool BLOCKING = true;

  protected:
    virtual uint64_t getMsgUtime(const T* msg)
    {
        return hasUtime<T>::utime(msg);
    }

    // The returned value must be "new" in all cases
    virtual T* interpolate(uint64_t utimeTarget,
                           const T* A, uint64_t utimeA,
                           const T* B, uint64_t utimeB)
    {
        return utimeTarget - utimeA < utimeB - utimeTarget ? new T(*A) : new T(*B);
    }

  private:
    zcm::ZCM* zcmLocal = nullptr;

    uint64_t maxTimeErr_us;

    std::atomic_bool done {false};

    std::deque<T*> buf;
    size_t bufMax;
    std::mutex bufLock;
    std::condition_variable newMsgCond;
    zcm::Subscription *s = nullptr;

    std::mutex callbackLock;
    std::condition_variable callbackCv;
    T* callbackMsg = nullptr;
    std::thread *thr = nullptr;
    callback onMsg;
    void* usr;

    Filter hzFilter;
    Filter jitterFilter;

    void callbackThreadFunc()
    {
        std::unique_lock<std::mutex> lk(callbackLock);
        while (!done) {
            callbackCv.wait(lk, [&](){ return callbackMsg || done; });
            if (done) return;
            onMsg(callbackMsg, getMsgUtime(callbackMsg), usr);
            // Intentionally not deleting callbackMsg as it is the
            // responsibility of the callback to delete the memory
            callbackMsg = nullptr;
        }
    }

    void handle(const zcm::ReceiveBuffer* rbuf, const std::string& chan, const T* _msg)
    {
        newMsg(_msg);
    }

    MessageTracker() {}

  public:
    MessageTracker(zcm::ZCM* zcmLocal, std::string channel,
                   double maxTimeErr = 0.25, size_t maxMsgs = 1,
                   callback onMsg = nullptr, void* usr = nullptr,
                   double freqEstConvergenceNumMsgs = 5)
        : zcmLocal(zcmLocal), maxTimeErr_us(maxTimeErr * 1e6), onMsg(onMsg), usr(usr),
              hzFilter(Filter::convergenceTimeToNatFreq(freqEstConvergenceNumMsgs, 0.8), 0.8),
          jitterFilter(Filter::convergenceTimeToNatFreq(freqEstConvergenceNumMsgs, 0.8), 0.8)
    {
        if (hasUtime<T>::present == true) {
            T tmp;
            std::string name = demangle(getType(tmp));
            ZCM_DEBUG("Message trackers using 'utime' field of zcmtype ", name);
        } else {
            T tmp;
            std::string name = demangle(getType(tmp));
            ZCM_DEBUG("Message trackers using local system receive utime for ",
                      "tracking zcmtype ", name);
        }

        bufMax = maxMsgs;

        if (onMsg != nullptr) thr = new std::thread(&MessageTracker<T>::callbackThreadFunc, this);
        if (zcmLocal && channel != "")
            s = zcmLocal->subscribe(channel, &MessageTracker<T>::handle, this);
    }

    virtual ~MessageTracker()
    {
        if (s) zcmLocal->unsubscribe(s);
        done = true;
        newMsgCond.notify_all();
        callbackCv.notify_all();
        if (thr) {
            thr->join();
            delete thr;
        }
        while (!buf.empty()) {
            T* tmp = buf.front();
            delete tmp;
            buf.pop_front();
        }
    }

    // You must free the memory returned here
    T* get(bool blocking = NONBLOCKING)
    {
        T* ret = nullptr;

        {
            std::unique_lock<std::mutex> lk(bufLock);
            if (blocking == BLOCKING) {
                if (buf.empty())
                    newMsgCond.wait(lk, [&]{ return !buf.empty() || done; });
                if (done) return nullptr;
            }
            if (!buf.empty())
                ret = new T(*buf.back());
        }

        return ret;
    }

    // This may return nullptr even in the blocking case
    // TODO: Should consider how to allow the user to ask for an extrapolated
    //       message if bracketted messages arent available
    T* get(uint64_t utime, bool blocking = NONBLOCKING)
    {
        std::unique_lock<std::mutex> lk(bufLock);

        if (blocking == BLOCKING) {
            // XXX This is technically not okay. "done" can't be an
            //     atomic as it can change after we've checked it in the
            //     wait. If it does change, and we go back to sleep,
            //     we'll never wake up. In other words, if done changes
            //     to true between the "if (done) return false;" and the
            //     end of the wait function, we would go back to sleep and
            //     never wake up again because this class would have already
            //     cleaned up
            newMsgCond.wait(lk, [&]{
                        if (done) return true;
                        if (buf.empty()) return false;
                        uint64_t recentUtime = getMsgUtime(buf.back());
                        if (recentUtime < utime) return false;
                        return true;
                    });
            if (done) return nullptr;
        }

        return get(utime, buf.begin(), buf.end(), lk);
    }

    // If you need to have a lock while working with the iterators, pass it in
    // here to have it unlocked once this function is done working with the iterators
    template <class InputIter>
    T* get(uint64_t utime, InputIter first, InputIter last, std::unique_lock<std::mutex>& lk)
    {
        T *m0 = nullptr, *m1 = nullptr; // two poses bracketing the desired utime
        uint64_t m0Utime = 0, m1Utime = UINT64_MAX;

        const T *_m0 = nullptr;
        const T *_m1 = nullptr;

        // The reason why we do a linear search is to support the ability
        // to skip around in logs. We want to support messages with
        // non-monitonically increasing utimes and this is the easiest way.
        // This can be made much faster if needed
        for (auto iter = first; iter != last; iter++) {
            const T* m = *iter;
            uint64_t mUtime = getMsgUtime(m);

            if (mUtime <= utime && (_m0 == nullptr || mUtime > m0Utime)) {
                _m0 = m;
                m0Utime = mUtime;
            }

            if (mUtime >= utime && (_m1 == nullptr || mUtime < m1Utime)) {
                _m1 = m;
                m1Utime = mUtime;
            }
        }

        if (_m0 != nullptr) m0 = new T(*_m0);
        if (_m1 != nullptr) m1 = new T(*_m1);

        if (lk.owns_lock()) lk.unlock();

        if (m0 && utime - m0Utime > maxTimeErr_us) {
            delete m0;
            m0 = nullptr;
        }

        if (m1 && m1Utime - utime > maxTimeErr_us) {
            delete m1;
            m1 = nullptr;
        }

        if (m0 && m1) {
            if (m0Utime == m1Utime) {
                delete m1;
                return m0;
            }

            T* elt = interpolate(utime, m0, m0Utime, m1, m1Utime);

            delete m0;
            delete m1;

            return elt;
        }

        if (m0) return m0;
        if (m1) return m1;

        return nullptr;
    }

    std::vector<T*> getRange(uint64_t utimeA, uint64_t utimeB, bool blocking = NONBLOCKING)
    {
        std::unique_lock<std::mutex> lk(bufLock);

        if (blocking == BLOCKING) {
            // XXX This is technically not okay. "done" can't be an
            //     atomic as it can change after we've checked it in the
            //     wait. If it does change, and we go back to sleep,
            //     we'll never wake up. In other words, if done changes
            //     to true between the "if (done) return false;" and the
            //     end of the wait function, we would go back to sleep and
            //     never wake up again because this class would have already
            //     cleaned up
            newMsgCond.wait(lk, [&]{
                        if (done) return true;
                        if (buf.empty()) return false;
                        uint64_t recentUtime = getMsgUtime(buf.back());
                        if (recentUtime < utimeA) return false;
                        return true;
                    });
            if (done) return nullptr;
        }

        uint64_t m0Utime = 0, m1Utime = UINT64_MAX;

        for (const T* m : buf) {
            uint64_t mUtime = getMsgUtime(m);
            if (mUtime <= utimeA && mUtime > m0Utime) m0Utime = mUtime;
            if (mUtime >= utimeB && mUtime < m1Utime) m1Utime = mUtime;
        }

        std::vector<T*> ret;
        for (const T* m : buf) {
            uint64_t mUtime = getMsgUtime(m);
            if (mUtime >= m0Utime && mUtime <= m1Utime) ret.push_back(new T(*m));
        }
        return ret;
    }

    void newMsg(const T* _msg)
    {
        if (done) return;

        T* tmp = new T(*_msg);
        {
            std::unique_lock<std::mutex> lk(bufLock);

            uint64_t lastUtime = 0;
            if (!buf.empty()) lastUtime = getMsgUtime(buf.back());

            // Expire due to buffer being full
            if (buf.size() == bufMax){
                T* tmp = buf.front();
                delete tmp;
                buf.pop_front();
            }

            // Expire things that are too old
            while (!buf.empty()) {
                if (getMsgUtime(buf.front()) + maxTimeErr_us > getMsgUtime(_msg)) break;
                T* tmp = buf.front();
                delete tmp;
                buf.pop_front();
            }

            if (lastUtime != 0) {
                double obs = getMsgUtime(tmp) - lastUtime;
                hzFilter(obs, 1);
                double jitterObs = hzFilter[Filter::HIGH_PASS];
                jitterFilter(jitterObs * jitterObs, 1);
            }

            buf.push_back(tmp);
        }
        newMsgCond.notify_all();

        if (callbackLock.try_lock()) {
            if (callbackMsg) delete callbackMsg;
            callbackMsg = new T(*_msg);
            callbackLock.unlock();
            callbackCv.notify_all();
        }
    }

    double getHz()
    {
        std::unique_lock<std::mutex> lk(bufLock);
        return 1e6 / hzFilter[Filter::LOW_PASS];
    }

    double getJitterUs()
    {
        std::unique_lock<std::mutex> lk(bufLock);
        return sqrt(jitterFilter[Filter::LOW_PASS]);
    }

  private:
    // *****************************************************************************
    // Insanely hacky trick to determine at compile time if a zcmtype has a
    // field called "utime"
    // TODO: I think c++11 has some utilities to make this a bit easier (I have not
    //       looked into them much at all, but worth looking at <type_traits>
    //       and what you can do with things like std::true_type and the like)
    template <typename F> struct hasUtime {
        struct Fallback { void* utime; }; // introduce member name "utime"
        struct Derived : F, Fallback {};

        template <typename C, C> struct ChT;

        template <typename C>
        static uint64_t _utime(ChT<void* Fallback::*, &C::utime>*, const F* msg)
        {
            struct timeval tv;
            gettimeofday(&tv, NULL);
            return (uint64_t)tv.tv_sec * 1000000 + tv.tv_usec;
        };
        template <typename C>
        static uint64_t _utime(void* tmp, ...)
        {
            va_list args;
            va_start(args, tmp);
            const F* msg = va_arg(args, const F*);
            va_end(args);
            return msg->utime;
        }

        template<typename C> static char (&f(ChT<int Fallback::*, &C::x>*))[1];
        template<typename C> static char (&f(...))[2];

        static bool const present = sizeof(f<Derived>(0)) == 2;

        static uint64_t const utime(const F* msg)
        {
            return _utime<Derived>(0,msg);
        }
    };

    template<typename F>
    static inline std::string getType(const F t) { return typeid(t).name(); }

    static inline std::string demangle(std::string name)
    {
        int status = -4; // some arbitrary value to eliminate the compiler warning
        std::unique_ptr<char, void(*)(void*)> res {
            abi::__cxa_demangle(name.c_str(), NULL, NULL, &status),
            std::free
        };
        return (status==0) ? res.get() : name ;
    }

    static void __ZCM_PRINT_OBFUSCATE__(std::ostream& o) { }

    template<typename First, typename ...Rest>
    static void __ZCM_PRINT_OBFUSCATE__(std::ostream& o, First && first, Rest && ...rest)
    {
        o << std::forward<First>(first);
        __ZCM_PRINT_OBFUSCATE__(o, std::forward<Rest>(rest)...);
    }
    // *****************************************************************************

};

}

#undef ZCM_DEBUG
<|endoftext|>


<commit_before>/* idmapping.cc			KPilot
**
** Copyright (C) 2007 by Bertjan Broeksema <b.broeksema@kdemail.net>
** Copyright (C) 2007 by Jason "vanRijn" Kasper <vr@movingparts.net>
*/
/*
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU Lesser General Public License as published by
** the Free Software Foundation; either version 2.1 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU Lesser General Public License for more details.
**
** You should have received a copy of the GNU Lesser General Public License
** along with this program in a file called COPYING; if not, write to
** the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
** MA 02110-1301, USA.
*/

/*
** Bug reports and questions can be sent to kde-pim@kde.org
*/

#include "idmapping.h"

#include <QtCore/QSet>
#include <QtCore/QtDebug>

#include "idmappingxmlsource.h"
#include "options.h"

class IDMappingPrivate : public QSharedData
{
public:
	IDMappingPrivate( const QString &userName, const QString &conduit )
		: fSource( userName, conduit )
	{
	}

	IDMappingPrivate()
	{
	}

	IDMappingPrivate( const IDMappingPrivate& other ) : QSharedData( other )
	{
		fSource = other.fSource;
	}

	IDMappingXmlSource fSource;
};

IDMapping::IDMapping() : d( new IDMappingPrivate )
{
}

IDMapping::IDMapping( const QString &userName, const QString &conduit )
	: d( new IDMappingPrivate( userName, conduit ) )
{
	FUNCTIONSETUP;

	d->fSource.loadMapping();
}

IDMapping::IDMapping( const IDMapping& other ) : d( other.d )
{
}

IDMapping::~IDMapping()
{
}

void IDMapping::archiveRecord( const QString &hhRecordId )
{
	FUNCTIONSETUP;

	if( containsHHId( hhRecordId ) )
	{
		QString pcRecordId = this->pcRecordId( hhRecordId );
		d->fSource.archivedRecords()->append( pcRecordId );
	}
}

void IDMapping::changeHHId( const QString &from, const QString &to )
{
	FUNCTIONSETUP;

	QString pcId = pcRecordId( from );
	d->fSource.mappings()->remove( from );
	d->fSource.mappings()->insert( to, pcId );
}

void IDMapping::changePCId( const QString &from, const QString &to )
{
	FUNCTIONSETUP;

	QString hhId = hhRecordId( from );
	d->fSource.mappings()->insert( hhId, to );
}

bool IDMapping::commit()
{
	FUNCTIONSETUP;

	return d->fSource.saveMapping();
}

bool IDMapping::containsHHId( const QString &recordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->contains( recordId );
}

bool IDMapping::containsPCId( const QString &recordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->values().contains( recordId );
}

QString IDMapping::hhCategory( const QString &hhRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.hhCategory( hhRecordId );
}

QString IDMapping::hhRecordId( const QString &id ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->key( id );
}

bool IDMapping::isArchivedRecord( const QString &pcRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constArchivedRecords()->contains( pcRecordId );
}

bool IDMapping::isValid( const QList<QString>& ids ) const
{
	FUNCTIONSETUP;

	const QMap<QString, QString>* mappings = d->fSource.constMappings();

	// If both are empty we have a valid mapping.
	if( ids.isEmpty() && mappings->isEmpty() )
	{
		return true;
	}

	DEBUGKPILOT << "At least one contains items.";

	// If both have different sizes then the mapping is surely not correct.
	if( ids.size() != mappings->size() )
	{
		DEBUGKPILOT << "Unequal size: given[" << ids.size() << "], mapping[" << mappings->size() << ']';
		return false;
	}

	// should be a 1..1 mapping between keys and values
	bool equalSize = (mappings->uniqueKeys().size() == mappings->size() );

	if( equalSize && !ids.isEmpty() )
	{
		bool idsInMapping = true;

		// now, check for validity of mappings.  *note* we can stop
		// looking if we find at least one problem.
		if( containsHHId( ids.first() ) )
		{
			foreach( const QString& storedId, d->fSource.constMappings()->keys() )
			{
				if( !ids.contains( storedId ) )
				{
					DEBUGKPILOT << "IDMapping::isValid(): HH Id " << storedId << " is in"
						<< " mapping but does not seem to occur in the give id list.";
					qDebug() << ids;
					return false;
				}
			}
		}
		else
		{
			// The ids are pc ids.
			foreach( const QString& storedId, d->fSource.constMappings()->values() )
			{
				if( !ids.contains( storedId ) )
				{
					DEBUGKPILOT << "IDMapping::isValid(): PC Id " << storedId << " is in"
						<< " mapping but does not seem to occur in the give id list.";
					return false;
				}
			}
		}

		// if we're otherwise valid, double-check and make sure that
		// we also have only unique values in our mapping...
		if (idsInMapping)
		{
			// build a unique list (set) of values and make sure we
			// have a 1..1 mapping
			QSet<QString> values = mappings->values().toSet();
			idsInMapping = (values.size() == mappings->size() );
			DEBUGKPILOT << "Reverse map integrity: ["
				<< idsInMapping << "]";
		}

		DEBUGKPILOT << "Returning: [" << idsInMapping << "]";

		return idsInMapping;
	}

	return false;
}

QDateTime IDMapping::lastSyncedDate() const
{
	return d->fSource.lastSyncedDate();
}

void IDMapping::map( const QString &hhRecordId, const QString &pcId )
{
	FUNCTIONSETUP;

	// check to see if we already have a key with this value
	QString existingHhRecordId = d->fSource.constMappings()->key( pcId );

	// if we already have a key for this one and it isn't the hhRecordId
	// that is being passed in, it's an error
	if( !existingHhRecordId.isEmpty() && existingHhRecordId != hhRecordId )
	{
		WARNINGKPILOT << "Error.  pcId:[" << pcId
			<< "] already mapped to hhRecordId: [" << existingHhRecordId
			<< "].  Should not have same pcId mapped also to incoming: ["
			<< hhRecordId << "].  Removing it.";
		d->fSource.mappings()->remove( existingHhRecordId );
	}

	d->fSource.mappings()->insert( hhRecordId, pcId );
}

QStringList IDMapping::pcCategories( const QString &pcRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.pcCategories( pcRecordId );
}

QString IDMapping::pcRecordId( const QString &id ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->value( id );
}

QStringList IDMapping::pcRecordIds() const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->values();
}

void IDMapping::removeHHId( const QString &recordId )
{
	FUNCTIONSETUP;

	d->fSource.mappings()->remove( recordId );
}

void IDMapping::removePCId( const QString &recordId )
{
	FUNCTIONSETUP;

	// The recordId is one of a pc record.
	QString hhId = d->fSource.mappings()->key( recordId );
	if( !hhId.isEmpty() )
	{
		d->fSource.mappings()->remove( hhId );
	}
}

bool IDMapping::rollback()
{
	FUNCTIONSETUP;

	return d->fSource.rollback();
}

void IDMapping::setLastSyncedDate( const QDateTime &dateTime )
{
	FUNCTIONSETUP;

	d->fSource.setLastSyncedDate( dateTime );
}

void IDMapping::setLastSyncedPC( const QString &pc )
{
	FUNCTIONSETUP;

	d->fSource.setLastSyncedPC( pc );
}

void IDMapping::storeHHCategory( const QString &hhRecordId
                               , const QString &category )
{
	FUNCTIONSETUP;

	if( containsHHId( hhRecordId ) )
	{
		d->fSource.setHHCategory( hhRecordId, category );
	}
}

void IDMapping::storePCCategories( const QString &pcRecordId
                                 , const QStringList &categories )
{
	FUNCTIONSETUP;

	if( containsPCId( pcRecordId ) )
	{
		d->fSource.setPCCategories( pcRecordId, categories );
	}
}

IDMapping& IDMapping::operator=( const IDMapping& other )
{
	if( this != &other )
	{
		d = other.d;
	}

	return *this;
}

bool IDMapping::remove()
{
	FUNCTIONSETUP;
	bool success = d->fSource.remove();
	return success && d->fSource.loadMapping(); // Make sure that we reset local structures.
}
<commit_msg>no need to call values, foreach iterates just fine over the values of a container without the extra memory allocation<commit_after>/* idmapping.cc			KPilot
**
** Copyright (C) 2007 by Bertjan Broeksema <b.broeksema@kdemail.net>
** Copyright (C) 2007 by Jason "vanRijn" Kasper <vr@movingparts.net>
*/
/*
** This program is free software; you can redistribute it and/or modify
** it under the terms of the GNU Lesser General Public License as published by
** the Free Software Foundation; either version 2.1 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU Lesser General Public License for more details.
**
** You should have received a copy of the GNU Lesser General Public License
** along with this program in a file called COPYING; if not, write to
** the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
** MA 02110-1301, USA.
*/

/*
** Bug reports and questions can be sent to kde-pim@kde.org
*/

#include "idmapping.h"

#include <QtCore/QSet>
#include <QtCore/QtDebug>

#include "idmappingxmlsource.h"
#include "options.h"

class IDMappingPrivate : public QSharedData
{
public:
	IDMappingPrivate( const QString &userName, const QString &conduit )
		: fSource( userName, conduit )
	{
	}

	IDMappingPrivate()
	{
	}

	IDMappingPrivate( const IDMappingPrivate& other ) : QSharedData( other )
	{
		fSource = other.fSource;
	}

	IDMappingXmlSource fSource;
};

IDMapping::IDMapping() : d( new IDMappingPrivate )
{
}

IDMapping::IDMapping( const QString &userName, const QString &conduit )
	: d( new IDMappingPrivate( userName, conduit ) )
{
	FUNCTIONSETUP;

	d->fSource.loadMapping();
}

IDMapping::IDMapping( const IDMapping& other ) : d( other.d )
{
}

IDMapping::~IDMapping()
{
}

void IDMapping::archiveRecord( const QString &hhRecordId )
{
	FUNCTIONSETUP;

	if( containsHHId( hhRecordId ) )
	{
		QString pcRecordId = this->pcRecordId( hhRecordId );
		d->fSource.archivedRecords()->append( pcRecordId );
	}
}

void IDMapping::changeHHId( const QString &from, const QString &to )
{
	FUNCTIONSETUP;

	QString pcId = pcRecordId( from );
	d->fSource.mappings()->remove( from );
	d->fSource.mappings()->insert( to, pcId );
}

void IDMapping::changePCId( const QString &from, const QString &to )
{
	FUNCTIONSETUP;

	QString hhId = hhRecordId( from );
	d->fSource.mappings()->insert( hhId, to );
}

bool IDMapping::commit()
{
	FUNCTIONSETUP;

	return d->fSource.saveMapping();
}

bool IDMapping::containsHHId( const QString &recordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->contains( recordId );
}

bool IDMapping::containsPCId( const QString &recordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->values().contains( recordId );
}

QString IDMapping::hhCategory( const QString &hhRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.hhCategory( hhRecordId );
}

QString IDMapping::hhRecordId( const QString &id ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->key( id );
}

bool IDMapping::isArchivedRecord( const QString &pcRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.constArchivedRecords()->contains( pcRecordId );
}

bool IDMapping::isValid( const QList<QString>& ids ) const
{
	FUNCTIONSETUP;

	const QMap<QString, QString>* mappings = d->fSource.constMappings();

	// If both are empty we have a valid mapping.
	if( ids.isEmpty() && mappings->isEmpty() )
	{
		return true;
	}

	DEBUGKPILOT << "At least one contains items.";

	// If both have different sizes then the mapping is surely not correct.
	if( ids.size() != mappings->size() )
	{
		DEBUGKPILOT << "Unequal size: given[" << ids.size() << "], mapping[" << mappings->size() << ']';
		return false;
	}

	// should be a 1..1 mapping between keys and values
	bool equalSize = (mappings->uniqueKeys().size() == mappings->size() );

	if( equalSize && !ids.isEmpty() )
	{
		bool idsInMapping = true;

		// now, check for validity of mappings.  *note* we can stop
		// looking if we find at least one problem.
		if( containsHHId( ids.first() ) )
		{
			foreach( const QString& storedId, d->fSource.constMappings()->keys() )
			{
				if( !ids.contains( storedId ) )
				{
					DEBUGKPILOT << "IDMapping::isValid(): HH Id " << storedId << " is in"
						<< " mapping but does not seem to occur in the give id list.";
					qDebug() << ids;
					return false;
				}
			}
		}
		else
		{
			// The ids are pc ids.
			foreach( const QString& storedId, *(d->fSource.constMappings()) )
			{
				if( !ids.contains( storedId ) )
				{
					DEBUGKPILOT << "IDMapping::isValid(): PC Id " << storedId << " is in"
						<< " mapping but does not seem to occur in the give id list.";
					return false;
				}
			}
		}

		// if we're otherwise valid, double-check and make sure that
		// we also have only unique values in our mapping...
		if (idsInMapping)
		{
			// build a unique list (set) of values and make sure we
			// have a 1..1 mapping
			QSet<QString> values = mappings->values().toSet();
			idsInMapping = (values.size() == mappings->size() );
			DEBUGKPILOT << "Reverse map integrity: ["
				<< idsInMapping << "]";
		}

		DEBUGKPILOT << "Returning: [" << idsInMapping << "]";

		return idsInMapping;
	}

	return false;
}

QDateTime IDMapping::lastSyncedDate() const
{
	return d->fSource.lastSyncedDate();
}

void IDMapping::map( const QString &hhRecordId, const QString &pcId )
{
	FUNCTIONSETUP;

	// check to see if we already have a key with this value
	QString existingHhRecordId = d->fSource.constMappings()->key( pcId );

	// if we already have a key for this one and it isn't the hhRecordId
	// that is being passed in, it's an error
	if( !existingHhRecordId.isEmpty() && existingHhRecordId != hhRecordId )
	{
		WARNINGKPILOT << "Error.  pcId:[" << pcId
			<< "] already mapped to hhRecordId: [" << existingHhRecordId
			<< "].  Should not have same pcId mapped also to incoming: ["
			<< hhRecordId << "].  Removing it.";
		d->fSource.mappings()->remove( existingHhRecordId );
	}

	d->fSource.mappings()->insert( hhRecordId, pcId );
}

QStringList IDMapping::pcCategories( const QString &pcRecordId ) const
{
	FUNCTIONSETUP;

	return d->fSource.pcCategories( pcRecordId );
}

QString IDMapping::pcRecordId( const QString &id ) const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->value( id );
}

QStringList IDMapping::pcRecordIds() const
{
	FUNCTIONSETUP;

	return d->fSource.constMappings()->values();
}

void IDMapping::removeHHId( const QString &recordId )
{
	FUNCTIONSETUP;

	d->fSource.mappings()->remove( recordId );
}

void IDMapping::removePCId( const QString &recordId )
{
	FUNCTIONSETUP;

	// The recordId is one of a pc record.
	QString hhId = d->fSource.mappings()->key( recordId );
	if( !hhId.isEmpty() )
	{
		d->fSource.mappings()->remove( hhId );
	}
}

bool IDMapping::rollback()
{
	FUNCTIONSETUP;

	return d->fSource.rollback();
}

void IDMapping::setLastSyncedDate( const QDateTime &dateTime )
{
	FUNCTIONSETUP;

	d->fSource.setLastSyncedDate( dateTime );
}

void IDMapping::setLastSyncedPC( const QString &pc )
{
	FUNCTIONSETUP;

	d->fSource.setLastSyncedPC( pc );
}

void IDMapping::storeHHCategory( const QString &hhRecordId
                               , const QString &category )
{
	FUNCTIONSETUP;

	if( containsHHId( hhRecordId ) )
	{
		d->fSource.setHHCategory( hhRecordId, category );
	}
}

void IDMapping::storePCCategories( const QString &pcRecordId
                                 , const QStringList &categories )
{
	FUNCTIONSETUP;

	if( containsPCId( pcRecordId ) )
	{
		d->fSource.setPCCategories( pcRecordId, categories );
	}
}

IDMapping& IDMapping::operator=( const IDMapping& other )
{
	if( this != &other )
	{
		d = other.d;
	}

	return *this;
}

bool IDMapping::remove()
{
	FUNCTIONSETUP;
	bool success = d->fSource.remove();
	return success && d->fSource.loadMapping(); // Make sure that we reset local structures.
}
<|endoftext|>


<commit_before>#include "mixture.h"
#include "utils.h"

#include <cmath>
using std::log;

#include <limits>
using std::numeric_limits;

#include <iostream>
using std::cout;
using std::endl;

#include <iomanip>
using std::setw;
using std::setprecision;

#include "Eigen/Core"
using Eigen::Dynamic;
using Eigen::Array;
using Eigen::ArrayXd;
using Eigen::ArrayXXd;
using Eigen::MatrixXd;

CMT::Mixture::Parameters::Parameters() :
	verbosity(1),
	maxIter(20),
	threshold(1e-5),
	valIter(2),
	valLookAhead(5),
	initialize(true),
	trainPriors(true),
	trainComponents(true),
	regularizePriors(0.)
{
}

CMT::Mixture::Component::Parameters::Parameters() :
	verbosity(0),
	maxIter(10),
	threshold(1e-5),
	trainPriors(true),
	trainCovariance(true),
	trainScales(true),
	trainMean(true),
	regularizePriors(0.),
	regularizeCovariance(0.),
	regularizeScales(0.),
	regularizeMean(0.)
{
}

void CMT::Mixture::Component::initialize(
	const MatrixXd& data,
	const Parameters& parameters)
{
}

CMT::Mixture::Mixture(int dim) : mDim(dim), mInitialized(false) {
}

CMT::Mixture::~Mixture() {
	for(int k = 0; k < mComponents.size(); ++k)
		delete mComponents[k];
}

void CMT::Mixture::addComponent(Component* component) {
	if(component->dim() != dim())
		throw Exception("Component has wrong dimensionality.");

	// add another parameter to prior weights vector
	VectorXd priors(numComponents() + 1);
	priors << mPriors * numComponents(), 1.;

	// renormalize
	mPriors = priors / (numComponents() + 1.);

	mComponents.push_back(component);
}

MatrixXd CMT::Mixture::sample(int numSamples) const {
	// cumulative distribution function
	ArrayXd cdf = mPriors;
	for(int k = 1; k < numComponents(); ++k)
		cdf[k] += cdf[k - 1];

	// make sure last entry is definitely large enough
	cdf[numComponents() - 1] = 1.0001;

	// initialize sample from multinomial distribution
	int numSamplesPerComp[numComponents()];
	for(int k = 0; k < numComponents(); ++k)
		numSamplesPerComp[k] = 0;

	// generate sample from multinomial distribution
	#pragma omp parallel for
	for(int i = 0; i < numSamples; ++i) {
		double urand = static_cast<double>(rand()) / (static_cast<long>(RAND_MAX) + 1l);

		int j = 0;
		while(urand > cdf[j])
			++j;

		numSamplesPerComp[j]++;
	}

	// container for samples from different components
	vector<MatrixXd> samples(numComponents());

	// sample each component
	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		samples[k] = mComponents[k]->sample(numSamplesPerComp[k]);

	return concatenate(samples);
}

ArrayXXd CMT::Mixture::posterior(const MatrixXd& data) {
	ArrayXXd logJoint(numComponents(), data.cols());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

	// return normalized posterior
	return (logJoint.rowwise() - logSumExp(logJoint)).exp();
}

Array<double, 1, Dynamic> CMT::Mixture::logLikelihood(const MatrixXd& data) const {
	ArrayXXd logJoint(numComponents(), data.cols());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

	return logSumExp(logJoint);
}

void CMT::Mixture::initialize(
	const MatrixXd& data,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.trainPriors)
		mPriors.setConstant(1. / numComponents());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		mComponents[k]->initialize(data, componentParameters);

	mInitialized = true;
}

bool CMT::Mixture::train(
	const MatrixXd& data,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.initialize && !initialized())
		initialize(data, parameters, componentParameters);

	ArrayXXd logJoint(numComponents(), data.cols());
	Array<double, Dynamic, 1> postSum;
	Array<double, 1, Dynamic> logLik;
	ArrayXXd post;
	ArrayXXd weights;
	double avgLogLoss = numeric_limits<double>::infinity();
	double avgLogLossNew;

	for(int i = 0; i < parameters.maxIter; ++i) {
		// compute joint probability of data and assignments (E)
		#pragma omp parallel for
		for(int k = 0; k < numComponents(); ++k)
			logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

		// compute normalized posterior (E)
		logLik = logSumExp(logJoint);

		// average negative log-likelihood in bits per component
		avgLogLossNew = -logLik.mean() / log(2.) / dim();

		if(parameters.verbosity > 0)
			cout << setw(6) << i << setw(14) << setprecision(7) << avgLogLossNew << endl;

		// test for convergence
		if(avgLogLoss - avgLogLossNew < parameters.threshold)
			return true;
		avgLogLoss = avgLogLossNew;

		// compute normalized posterior (E)
		post = (logJoint.rowwise() - logLik).exp();
		postSum = post.rowwise().sum();
		weights = post.colwise() / postSum;

		// optimize prior weights (M)
		if(parameters.trainPriors) {
			mPriors = postSum / data.cols() + parameters.regularizePriors;
			mPriors /= mPriors.sum();
		}

		// optimize components (M)
		if(parameters.trainComponents) {
			#pragma omp parallel for
			for(int k = 0; k < numComponents(); ++k)
				mComponents[k]->train(data, weights.row(k), componentParameters);
		} else {
			return true;
		}
	}

	if(parameters.verbosity > 0)
		cout << setw(6) << parameters.maxIter << setw(14) << setprecision(7) << evaluate(data) << endl;

	return false;
}

bool CMT::Mixture::train(
	const MatrixXd& data,
	const MatrixXd& dataValid,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.initialize && !initialized())
		initialize(data, parameters, componentParameters);

	ArrayXXd logJoint(numComponents(), data.cols());
	Array<double, Dynamic, 1> postSum;
	Array<double, 1, Dynamic> logLik;
	ArrayXXd post;
	ArrayXXd weights;

	// training and validation log-loss for checking convergence
	double avgLogLoss = numeric_limits<double>::infinity();
	double avgLogLossNew;
	double avgLogLossValid = evaluate(dataValid);
	double avgLogLossValidNew = avgLogLossValid;
	int counter = 0;

	// backup model parameters
	VectorXd priors = mPriors;
	vector<Component*> components;

	for(int k = 0; k < numComponents(); ++k)
		components.push_back(mComponents[k]->copy());

	for(int i = 0; i < parameters.maxIter; ++i) {
		// compute joint probability of data and assignments (E)
		#pragma omp parallel for
		for(int k = 0; k < numComponents(); ++k)
			logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

		// compute normalized posterior (E)
		logLik = logSumExp(logJoint);

		// average negative log-likelihood in bits per component
		avgLogLossNew = -logLik.mean() / log(2.) / dim();

		if(parameters.verbosity > 0) {
			if(i % parameters.valIter == 0) {
				// print training and validation error
				cout << setw(6) << i;
				cout << setw(14) << setprecision(7) << avgLogLossNew;
				cout << setw(14) << setprecision(7) << avgLogLossValidNew << endl;
			} else {
				// print training error
				cout << setw(6) << i << setw(14) << setprecision(7) << avgLogLossNew << endl;
			}
		}

		// test for convergence
		if(avgLogLoss - avgLogLossNew < parameters.threshold)
			return true;
		avgLogLoss = avgLogLossNew;

		// compute normalized posterior (E)
		post = (logJoint.rowwise() - logLik).exp();
		postSum = post.rowwise().sum();
		weights = post.colwise() / postSum;

		// optimize prior weights (M)
		if(parameters.trainPriors) {
			mPriors = postSum / data.cols() + parameters.regularizePriors;
			mPriors /= mPriors.sum();
		}

		// optimize components (M)
		if(parameters.trainComponents) {
			#pragma omp parallel for
			for(int k = 0; k < numComponents(); ++k)
				mComponents[k]->train(data, weights.row(k), componentParameters);
		} else {
			return true;
		}

		if((i + 1) % parameters.valIter == 0) {
			// check validation error
			avgLogLossValidNew = evaluate(dataValid);

			if(avgLogLossValidNew < avgLogLossValid) {
				// backup new found model parameters
				priors = mPriors;
				for(int k = 0; k < numComponents(); ++k)
					*components[k] = *mComponents[k];

				avgLogLossValid = avgLogLossValidNew;
			} else {
				counter++;

				if(parameters.valLookAhead > 0 && counter >= parameters.valLookAhead) {
					// set parameters to best parameters found during training
					mPriors = priors;

					for(int k = 0; k < numComponents(); ++k) {
						*mComponents[k] = *components[k];
						delete components[k];
					}

					return true;
				}
			}
		}
	}

	if(parameters.verbosity > 0)
		cout << setw(6) << parameters.maxIter << setw(11) << setprecision(5) << evaluate(data) << endl;

	return false;
}
<commit_msg>Minor change.<commit_after>#include "mixture.h"
#include "utils.h"

#include <cmath>
using std::log;

#include <limits>
using std::numeric_limits;

#include <iostream>
using std::cout;
using std::endl;

#include <iomanip>
using std::setw;
using std::setprecision;

#include "Eigen/Core"
using Eigen::Dynamic;
using Eigen::Array;
using Eigen::ArrayXd;
using Eigen::ArrayXXd;
using Eigen::MatrixXd;

CMT::Mixture::Parameters::Parameters() :
	verbosity(1),
	maxIter(20),
	threshold(1e-5),
	valIter(2),
	valLookAhead(5),
	initialize(true),
	trainPriors(true),
	trainComponents(true),
	regularizePriors(0.)
{
}

CMT::Mixture::Component::Parameters::Parameters() :
	verbosity(0),
	maxIter(10),
	threshold(1e-5),
	trainPriors(true),
	trainCovariance(true),
	trainScales(true),
	trainMean(true),
	regularizePriors(0.),
	regularizeCovariance(0.),
	regularizeScales(0.),
	regularizeMean(0.)
{
}

void CMT::Mixture::Component::initialize(
	const MatrixXd& data,
	const Parameters& parameters)
{
}

CMT::Mixture::Mixture(int dim) : mDim(dim), mInitialized(false) {
}

CMT::Mixture::~Mixture() {
	for(int k = 0; k < mComponents.size(); ++k)
		delete mComponents[k];
}

void CMT::Mixture::addComponent(Component* component) {
	if(component->dim() != dim())
		throw Exception("Component has wrong dimensionality.");

	// add another parameter to prior weights vector
	VectorXd priors(numComponents() + 1);
	priors << mPriors * numComponents(), 1.;

	// renormalize
	mPriors = priors / (numComponents() + 1.);

	mComponents.push_back(component);
}

MatrixXd CMT::Mixture::sample(int numSamples) const {
	// cumulative distribution function
	ArrayXd cdf = mPriors;
	for(int k = 1; k < numComponents(); ++k)
		cdf[k] += cdf[k - 1];

	// make sure last entry is definitely large enough
	cdf[numComponents() - 1] = 1.0001;

	// initialize sample from multinomial distribution
	int numSamplesPerComp[numComponents()];
	for(int k = 0; k < numComponents(); ++k)
		numSamplesPerComp[k] = 0;

	// generate sample from multinomial distribution
	#pragma omp parallel for
	for(int i = 0; i < numSamples; ++i) {
		double urand = static_cast<double>(rand()) / RAND_MAX;

		int j = 0;
		while(urand > cdf[j])
			++j;

		numSamplesPerComp[j]++;
	}

	// container for samples from different components
	vector<MatrixXd> samples(numComponents());

	// sample each component
	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		samples[k] = mComponents[k]->sample(numSamplesPerComp[k]);

	return concatenate(samples);
}

ArrayXXd CMT::Mixture::posterior(const MatrixXd& data) {
	ArrayXXd logJoint(numComponents(), data.cols());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

	// return normalized posterior
	return (logJoint.rowwise() - logSumExp(logJoint)).exp();
}

Array<double, 1, Dynamic> CMT::Mixture::logLikelihood(const MatrixXd& data) const {
	ArrayXXd logJoint(numComponents(), data.cols());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

	return logSumExp(logJoint);
}

void CMT::Mixture::initialize(
	const MatrixXd& data,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.trainPriors)
		mPriors.setConstant(1. / numComponents());

	#pragma omp parallel for
	for(int k = 0; k < numComponents(); ++k)
		mComponents[k]->initialize(data, componentParameters);

	mInitialized = true;
}

bool CMT::Mixture::train(
	const MatrixXd& data,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.initialize && !initialized())
		initialize(data, parameters, componentParameters);

	ArrayXXd logJoint(numComponents(), data.cols());
	Array<double, Dynamic, 1> postSum;
	Array<double, 1, Dynamic> logLik;
	ArrayXXd post;
	ArrayXXd weights;
	double avgLogLoss = numeric_limits<double>::infinity();
	double avgLogLossNew;

	for(int i = 0; i < parameters.maxIter; ++i) {
		// compute joint probability of data and assignments (E)
		#pragma omp parallel for
		for(int k = 0; k < numComponents(); ++k)
			logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

		// compute normalized posterior (E)
		logLik = logSumExp(logJoint);

		// average negative log-likelihood in bits per component
		avgLogLossNew = -logLik.mean() / log(2.) / dim();

		if(parameters.verbosity > 0)
			cout << setw(6) << i << setw(14) << setprecision(7) << avgLogLossNew << endl;

		// test for convergence
		if(avgLogLoss - avgLogLossNew < parameters.threshold)
			return true;
		avgLogLoss = avgLogLossNew;

		// compute normalized posterior (E)
		post = (logJoint.rowwise() - logLik).exp();
		postSum = post.rowwise().sum();
		weights = post.colwise() / postSum;

		// optimize prior weights (M)
		if(parameters.trainPriors) {
			mPriors = postSum / data.cols() + parameters.regularizePriors;
			mPriors /= mPriors.sum();
		}

		// optimize components (M)
		if(parameters.trainComponents) {
			#pragma omp parallel for
			for(int k = 0; k < numComponents(); ++k)
				mComponents[k]->train(data, weights.row(k), componentParameters);
		} else {
			return true;
		}
	}

	if(parameters.verbosity > 0)
		cout << setw(6) << parameters.maxIter << setw(14) << setprecision(7) << evaluate(data) << endl;

	return false;
}

bool CMT::Mixture::train(
	const MatrixXd& data,
	const MatrixXd& dataValid,
	const Parameters& parameters,
	const Component::Parameters& componentParameters)
{
	if(parameters.initialize && !initialized())
		initialize(data, parameters, componentParameters);

	ArrayXXd logJoint(numComponents(), data.cols());
	Array<double, Dynamic, 1> postSum;
	Array<double, 1, Dynamic> logLik;
	ArrayXXd post;
	ArrayXXd weights;

	// training and validation log-loss for checking convergence
	double avgLogLoss = numeric_limits<double>::infinity();
	double avgLogLossNew;
	double avgLogLossValid = evaluate(dataValid);
	double avgLogLossValidNew = avgLogLossValid;
	int counter = 0;

	// backup model parameters
	VectorXd priors = mPriors;
	vector<Component*> components;

	for(int k = 0; k < numComponents(); ++k)
		components.push_back(mComponents[k]->copy());

	for(int i = 0; i < parameters.maxIter; ++i) {
		// compute joint probability of data and assignments (E)
		#pragma omp parallel for
		for(int k = 0; k < numComponents(); ++k)
			logJoint.row(k) = mComponents[k]->logLikelihood(data) + log(mPriors[k]);

		// compute normalized posterior (E)
		logLik = logSumExp(logJoint);

		// average negative log-likelihood in bits per component
		avgLogLossNew = -logLik.mean() / log(2.) / dim();

		if(parameters.verbosity > 0) {
			if(i % parameters.valIter == 0) {
				// print training and validation error
				cout << setw(6) << i;
				cout << setw(14) << setprecision(7) << avgLogLossNew;
				cout << setw(14) << setprecision(7) << avgLogLossValidNew << endl;
			} else {
				// print training error
				cout << setw(6) << i << setw(14) << setprecision(7) << avgLogLossNew << endl;
			}
		}

		// test for convergence
		if(avgLogLoss - avgLogLossNew < parameters.threshold)
			return true;
		avgLogLoss = avgLogLossNew;

		// compute normalized posterior (E)
		post = (logJoint.rowwise() - logLik).exp();
		postSum = post.rowwise().sum();
		weights = post.colwise() / postSum;

		// optimize prior weights (M)
		if(parameters.trainPriors) {
			mPriors = postSum / data.cols() + parameters.regularizePriors;
			mPriors /= mPriors.sum();
		}

		// optimize components (M)
		if(parameters.trainComponents) {
			#pragma omp parallel for
			for(int k = 0; k < numComponents(); ++k)
				mComponents[k]->train(data, weights.row(k), componentParameters);
		} else {
			return true;
		}

		if((i + 1) % parameters.valIter == 0) {
			// check validation error
			avgLogLossValidNew = evaluate(dataValid);

			if(avgLogLossValidNew < avgLogLossValid) {
				// backup new found model parameters
				priors = mPriors;
				for(int k = 0; k < numComponents(); ++k)
					*components[k] = *mComponents[k];

				avgLogLossValid = avgLogLossValidNew;
			} else {
				counter++;

				if(parameters.valLookAhead > 0 && counter >= parameters.valLookAhead) {
					// set parameters to best parameters found during training
					mPriors = priors;

					for(int k = 0; k < numComponents(); ++k) {
						*mComponents[k] = *components[k];
						delete components[k];
					}

					return true;
				}
			}
		}
	}

	if(parameters.verbosity > 0)
		cout << setw(6) << parameters.maxIter << setw(11) << setprecision(5) << evaluate(data) << endl;

	return false;
}
<|endoftext|>


<commit_before>
#include "ksp_plugin/interface.hpp"

#include "base/array.hpp"
#include "base/status.hpp"
#include "base/status_or.hpp"
#include "journal/method.hpp"
#include "journal/profiles.hpp"
#include "physics/apsides.hpp"

namespace principia {
namespace interface {

using base::Error;
using base::StatusOr;
using base::UniqueBytes;
using geometry::AngularVelocity;
using ksp_plugin::FlightPlan;
using ksp_plugin::Navigation;
using ksp_plugin::Vessel;
using physics::BodyCentredNonRotatingDynamicFrame;
using physics::ComputeApsides;
using physics::DiscreteTrajectory;
using physics::RigidMotion;
using physics::RigidTransformation;

namespace {

// This is called "New" because it will eventually construct and release the
// message string.  For now it just wraps the error code.
Status NewStatus(base::Status status) {
  Status result;
  if (!status.ok()) {
    LOG(ERROR) << status.message();
  }
  result.error = static_cast<int>(status.error());
  return result;
}

// A wrapper for |NewStatus(base::Status(error, message))|, since we often
// construct the status in the interface itself.
Status NewStatus(Error const error, std::string const& message) {
  return NewStatus(base::Status(error, message));
}

Status OK() {
  return NewStatus(base::Status::OK);
}

}  // namespace

Status principia__ExternalFlowFreefall(
    Plugin const* const plugin,
    int const central_body_index,
    QP const world_body_centred_initial_degrees_of_freedom,
    double const t_initial,
    double const t_final,
    QP* const world_body_centred_final_degrees_of_freedom) {
  journal::Method<journal::ExternalFlowFreefall> m{
      {plugin,
       central_body_index,
       world_body_centred_initial_degrees_of_freedom,
       t_initial,
       t_final},
      {world_body_centred_final_degrees_of_freedom}};
  if (plugin == nullptr) {
    return m.Return(
        NewStatus(Error::INVALID_ARGUMENT, "|plugin| must not be null"));
  }
  return m.Return(NewStatus(Error::UNIMPLEMENTED,
                            "|ExternalFlowFreefall| is not yet implemented"));
}

Status principia__ExternalGetNearestPlannedCoastDegreesOfFreedom(
    Plugin const* const plugin,
    int const central_body_index,
    char const* const vessel_guid,
    int manoeuvre_index,
    XYZ world_body_centred_reference_position,
    QP* world_body_centred_nearest_degrees_of_freedom) {
  journal::Method<journal::ExternalGetNearestPlannedCoastDegreesOfFreedom> m{
      {plugin,
       central_body_index,
       vessel_guid,
       manoeuvre_index,
       world_body_centred_reference_position},
      {world_body_centred_nearest_degrees_of_freedom}};
  if (plugin == nullptr) {
    return m.Return(
        NewStatus(Error::INVALID_ARGUMENT, "|plugin| must not be null"));
  }
  if (manoeuvre_index < 0) {
    return m.Return(NewStatus(Error::INVALID_ARGUMENT,
                              "Invalid negative |manoeuvre_index|" +
                                  std::to_string(manoeuvre_index)));
  }
  if (!plugin->HasCelestial(central_body_index)) {
    return m.Return(NewStatus(
        Error::NOT_FOUND,
        "No celestial with index " + std::to_string(central_body_index)));
  }
  if (!plugin->HasVessel(vessel_guid)) {
    return m.Return(NewStatus(
        Error::NOT_FOUND,
        "No vessel with GUID " + std::string(vessel_guid)));
  }
  Vessel const& vessel = *plugin->GetVessel(vessel_guid);
  if (!vessel.has_flight_plan()) {
    return m.Return(NewStatus(
        Error::FAILED_PRECONDITION,
        "Vessel " + vessel.ShortDebugString() + " has no flight plan"));
  }
  FlightPlan const& flight_plan = vessel.flight_plan();
  if (manoeuvre_index >= flight_plan.number_of_manuvres()) {
    return m.Return(NewStatus(
        Error::OUT_OF_RANGE,
        "|manoeuvre_index| " + std::to_string(manoeuvre_index) +
            " out of range, vessel " + vessel.ShortDebugString() + " has " +
            std::to_string(flight_plan.number_of_manuvres()) +
            u8" planned manuvres"));
  }
  // The index of the coast segment following the desired manuvre.
  int const segment_index = manoeuvre_index * 2 + 3;
  if (segment_index >= flight_plan.number_of_segments()) {
    return m.Return(NewStatus(Error::FAILED_PRECONDITION,
                              u8"A singularity occurs within manuvre " +
                                  std::to_string(manoeuvre_index) + " of " +
                                  vessel.ShortDebugString()));
  }
  DiscreteTrajectory<Barycentric>::Iterator coast_begin;
  DiscreteTrajectory<Barycentric>::Iterator coast_end;
  flight_plan.GetSegment(segment_index, coast_begin, coast_end);
  auto const body_centred_inertial =
      plugin->NewBodyCentredNonRotatingNavigationFrame(central_body_index);
  DiscreteTrajectory<Navigation> coast;
  for (auto it = coast_begin; it != coast_end; ++it) {
    coast.Append(it.time(),
                 body_centred_inertial->ToThisFrameAtTime(it.time())(
                     it.degrees_of_freedom()));
  }

  Instant const current_time = plugin->CurrentTime();
  // The given |World| position and requested |World| degrees of freedom are
  // body-centred inertial, so |body_centred_inertial| up to an orthogonal map
  // to world coordinates.  Do the conversion directly.
  // NOTE(eggrobin): it is correct to use the orthogonal map at |current_time|,
  // because |body_centred_inertial| does not rotate with respect to
  // |Barycentric|, so the orthogonal map does not depend on time.
  RigidMotion<Navigation, World> to_world_body_centred_inertial(
      RigidTransformation<Navigation, World>(
          Navigation::origin,
          World::origin,
          plugin->renderer().BarycentricToWorld(plugin->PlanetariumRotation()) *
              body_centred_inertial->FromThisFrameAtTime(
                  current_time).orthogonal_map()),
      AngularVelocity<Navigation>{},
      Velocity<Navigation>{});
  auto const from_world_body_centred_inertial =
      to_world_body_centred_inertial.Inverse();
  Position<Navigation> reference_position =
      from_world_body_centred_inertial.rigid_transformation()(
          FromXYZ<Position<World>>(world_body_centred_reference_position));
  DiscreteTrajectory<Navigation> immobile_reference;
  immobile_reference.Append(coast.Begin().time(),
                            {reference_position, Velocity<Navigation>{}});
  if (coast.Begin() !=
      coast.last()) {
    immobile_reference.Append(coast.last().time(),
                              {reference_position, Velocity<Navigation>{}});
  }
  DiscreteTrajectory<Navigation> apoapsides;
  DiscreteTrajectory<Navigation> periapsides;
  ComputeApsides(/*reference=*/immobile_reference,
                 coast.Begin(),
                 coast.End(),
                 apoapsides,
                 periapsides);
  if (periapsides.Empty()) {
    bool const coasting_away =
        (coast.Begin().degrees_of_freedom().position() -
         reference_position).Norm() <
        (coast.last().degrees_of_freedom().position() -
         reference_position).Norm();
    *world_body_centred_nearest_degrees_of_freedom =
        ToQP(to_world_body_centred_inertial(
            coasting_away ? coast.Begin().degrees_of_freedom()
                          : coast.last().degrees_of_freedom()));
  } else {
    *world_body_centred_nearest_degrees_of_freedom =
        ToQP(to_world_body_centred_inertial(
            periapsides.Begin().degrees_of_freedom()));
  }
  return m.Return(OK());
}

}  // namespace interface
}  // namespace principia
<commit_msg>utf-8<commit_after>
#include "ksp_plugin/interface.hpp"

#include "base/array.hpp"
#include "base/status.hpp"
#include "base/status_or.hpp"
#include "journal/method.hpp"
#include "journal/profiles.hpp"
#include "physics/apsides.hpp"

namespace principia {
namespace interface {

using base::Error;
using base::StatusOr;
using base::UniqueBytes;
using geometry::AngularVelocity;
using ksp_plugin::FlightPlan;
using ksp_plugin::Navigation;
using ksp_plugin::Vessel;
using physics::BodyCentredNonRotatingDynamicFrame;
using physics::ComputeApsides;
using physics::DiscreteTrajectory;
using physics::RigidMotion;
using physics::RigidTransformation;

namespace {

// This is called "New" because it will eventually construct and release the
// message string.  For now it just wraps the error code.
Status NewStatus(base::Status status) {
  Status result;
  if (!status.ok()) {
    LOG(ERROR) << status.message();
  }
  result.error = static_cast<int>(status.error());
  return result;
}

// A wrapper for |NewStatus(base::Status(error, message))|, since we often
// construct the status in the interface itself.
Status NewStatus(Error const error, std::string const& message) {
  return NewStatus(base::Status(error, message));
}

Status OK() {
  return NewStatus(base::Status::OK);
}

}  // namespace

Status principia__ExternalFlowFreefall(
    Plugin const* const plugin,
    int const central_body_index,
    QP const world_body_centred_initial_degrees_of_freedom,
    double const t_initial,
    double const t_final,
    QP* const world_body_centred_final_degrees_of_freedom) {
  journal::Method<journal::ExternalFlowFreefall> m{
      {plugin,
       central_body_index,
       world_body_centred_initial_degrees_of_freedom,
       t_initial,
       t_final},
      {world_body_centred_final_degrees_of_freedom}};
  if (plugin == nullptr) {
    return m.Return(
        NewStatus(Error::INVALID_ARGUMENT, "|plugin| must not be null"));
  }
  return m.Return(NewStatus(Error::UNIMPLEMENTED,
                            "|ExternalFlowFreefall| is not yet implemented"));
}

Status principia__ExternalGetNearestPlannedCoastDegreesOfFreedom(
    Plugin const* const plugin,
    int const central_body_index,
    char const* const vessel_guid,
    int manoeuvre_index,
    XYZ world_body_centred_reference_position,
    QP* world_body_centred_nearest_degrees_of_freedom) {
  journal::Method<journal::ExternalGetNearestPlannedCoastDegreesOfFreedom> m{
      {plugin,
       central_body_index,
       vessel_guid,
       manoeuvre_index,
       world_body_centred_reference_position},
      {world_body_centred_nearest_degrees_of_freedom}};
  if (plugin == nullptr) {
    return m.Return(
        NewStatus(Error::INVALID_ARGUMENT, "|plugin| must not be null"));
  }
  if (manoeuvre_index < 0) {
    return m.Return(NewStatus(Error::INVALID_ARGUMENT,
                              "Invalid negative |manoeuvre_index|" +
                                  std::to_string(manoeuvre_index)));
  }
  if (!plugin->HasCelestial(central_body_index)) {
    return m.Return(NewStatus(
        Error::NOT_FOUND,
        "No celestial with index " + std::to_string(central_body_index)));
  }
  if (!plugin->HasVessel(vessel_guid)) {
    return m.Return(NewStatus(
        Error::NOT_FOUND,
        "No vessel with GUID " + std::string(vessel_guid)));
  }
  Vessel const& vessel = *plugin->GetVessel(vessel_guid);
  if (!vessel.has_flight_plan()) {
    return m.Return(NewStatus(
        Error::FAILED_PRECONDITION,
        "Vessel " + vessel.ShortDebugString() + " has no flight plan"));
  }
  FlightPlan const& flight_plan = vessel.flight_plan();
  if (manoeuvre_index >= flight_plan.number_of_manœuvres()) {
    return m.Return(NewStatus(
        Error::OUT_OF_RANGE,
        "|manoeuvre_index| " + std::to_string(manoeuvre_index) +
            " out of range, vessel " + vessel.ShortDebugString() + " has " +
            std::to_string(flight_plan.number_of_manœuvres()) +
            u8" planned manœuvres"));
  }
  // The index of the coast segment following the desired manœuvre.
  int const segment_index = manoeuvre_index * 2 + 3;
  if (segment_index >= flight_plan.number_of_segments()) {
    return m.Return(NewStatus(Error::FAILED_PRECONDITION,
                              u8"A singularity occurs within manœuvre " +
                                  std::to_string(manoeuvre_index) + " of " +
                                  vessel.ShortDebugString()));
  }
  DiscreteTrajectory<Barycentric>::Iterator coast_begin;
  DiscreteTrajectory<Barycentric>::Iterator coast_end;
  flight_plan.GetSegment(segment_index, coast_begin, coast_end);
  auto const body_centred_inertial =
      plugin->NewBodyCentredNonRotatingNavigationFrame(central_body_index);
  DiscreteTrajectory<Navigation> coast;
  for (auto it = coast_begin; it != coast_end; ++it) {
    coast.Append(it.time(),
                 body_centred_inertial->ToThisFrameAtTime(it.time())(
                     it.degrees_of_freedom()));
  }

  Instant const current_time = plugin->CurrentTime();
  // The given |World| position and requested |World| degrees of freedom are
  // body-centred inertial, so |body_centred_inertial| up to an orthogonal map
  // to world coordinates.  Do the conversion directly.
  // NOTE(eggrobin): it is correct to use the orthogonal map at |current_time|,
  // because |body_centred_inertial| does not rotate with respect to
  // |Barycentric|, so the orthogonal map does not depend on time.
  RigidMotion<Navigation, World> to_world_body_centred_inertial(
      RigidTransformation<Navigation, World>(
          Navigation::origin,
          World::origin,
          plugin->renderer().BarycentricToWorld(plugin->PlanetariumRotation()) *
              body_centred_inertial->FromThisFrameAtTime(
                  current_time).orthogonal_map()),
      AngularVelocity<Navigation>{},
      Velocity<Navigation>{});
  auto const from_world_body_centred_inertial =
      to_world_body_centred_inertial.Inverse();
  Position<Navigation> reference_position =
      from_world_body_centred_inertial.rigid_transformation()(
          FromXYZ<Position<World>>(world_body_centred_reference_position));
  DiscreteTrajectory<Navigation> immobile_reference;
  immobile_reference.Append(coast.Begin().time(),
                            {reference_position, Velocity<Navigation>{}});
  if (coast.Begin() !=
      coast.last()) {
    immobile_reference.Append(coast.last().time(),
                              {reference_position, Velocity<Navigation>{}});
  }
  DiscreteTrajectory<Navigation> apoapsides;
  DiscreteTrajectory<Navigation> periapsides;
  ComputeApsides(/*reference=*/immobile_reference,
                 coast.Begin(),
                 coast.End(),
                 apoapsides,
                 periapsides);
  if (periapsides.Empty()) {
    bool const coasting_away =
        (coast.Begin().degrees_of_freedom().position() -
         reference_position).Norm²() <
        (coast.last().degrees_of_freedom().position() -
         reference_position).Norm²();
    *world_body_centred_nearest_degrees_of_freedom =
        ToQP(to_world_body_centred_inertial(
            coasting_away ? coast.Begin().degrees_of_freedom()
                          : coast.last().degrees_of_freedom()));
  } else {
    *world_body_centred_nearest_degrees_of_freedom =
        ToQP(to_world_body_centred_inertial(
            periapsides.Begin().degrees_of_freedom()));
  }
  return m.Return(OK());
}

}  // namespace interface
}  // namespace principia
<|endoftext|>


<commit_before>#include <string.h>
#include "frogboy.h"
#include "game.h"
#include "entity.h"
#include "bullet.h"
#include "critter.h"
#include "player.h"
#include "particle.h"
#include "sprites_bitmap.h"
#include "map.h"

bool pausePressed;
bool gamePaused;

const char pauseMessage[] FROGBOY_ROM_DATA = "PAUSED";

uint8_t wipeProgress;
const uint8_t wipeMasks[] FROGBOY_ROM_DATA = {0x00, 0x01, 0x11, 0x51, 0x55, 0xD5, 0xDD, 0xDF, 0xFF};

const uint8_t doorData[DOOR_TYPE_COUNT * 3] FROGBOY_ROM_DATA = {
    2, 2, MAP_TYPE_GRASSLAND,
    2, 2, MAP_TYPE_HOUSE,
    10, 1, MAP_TYPE_GRASSLAND,
    5, 2, MAP_TYPE_HOUSE2,
    40, 1, MAP_TYPE_GRASSLAND,
    2, 2, MAP_TYPE_HOUSE3,
    64, 2, MAP_TYPE_GRASSLAND,
};

void gameInit() {
    playerInitSystem();
    mapInitSystem();

    gameEnterDoor(DOOR_TYPE_START);

    playerUpdate();
    gamePaused = false;
    pausePressed = true;
}

void gameDraw() {
    mapDraw();
    critterDrawAll();
    playerDraw();
    bulletDrawAll();
    particleDrawAll();

    uint8_t mask = frogboy::readRom<uint8_t>(&wipeMasks[wipeProgress >> 2]);
    uint8_t* buffer = frogboy::getScreenBuffer();
    for(uint16_t i = 0; i != frogboy::SCREEN_WIDTH * (frogboy::SCREEN_HEIGHT / 8); ++i) {
        *buffer = *buffer & mask;
        buffer++;
    }

    if(gamePaused) {
        for(uint8_t i = 0; i != sizeof(pauseMessage) + 1; ++i) {
            frogboy::drawTile(32 + i * 8, 24, spritesBitmap, 0x4A, 0, false, false);
        }
        frogboy::printRomString(40, 24, spritesBitmap, pauseMessage, 1);
    }
    playerDrawHUD();
}

enum {
    CAMERA_BORDER = 6,
    CAMERA_MAX_SPEED = 4,
};

bool gameCheckOnScreen(int16_t x, int16_t y, uint8_t borderX, uint8_t borderY) {
    return x - mapCameraX < 128 + borderX
        && x - mapCameraX > -borderX
        && y - mapCameraY < 64 + borderY
        && y - mapCameraY > -borderY;
}

void gameUpdateCamera() {
    int16_t playerX = ents[ENT_OFFSET_PLAYER].x / 16 + 8;
    if(playerX < mapCameraX + frogboy::SCREEN_WIDTH / 2 - CAMERA_BORDER) {
        int16_t distance = mapCameraX + frogboy::SCREEN_WIDTH / 2 - CAMERA_BORDER - playerX;
        if(distance > CAMERA_MAX_SPEED) {
            distance = CAMERA_MAX_SPEED;
        }
        mapCameraX -= distance;
    }
    if(playerX > mapCameraX + frogboy::SCREEN_WIDTH / 2 + CAMERA_BORDER) {
        int16_t distance = playerX - (mapCameraX + frogboy::SCREEN_WIDTH / 2 + CAMERA_BORDER);
        if(distance > CAMERA_MAX_SPEED) {
            distance = CAMERA_MAX_SPEED;
        }
        mapCameraX += distance;
    }

    if(mapCameraX <= 0) {
        mapCameraX = 0;
    }
    if(mapCameraX >= mapGetWidth() * 16 - frogboy::SCREEN_WIDTH) {
        mapCameraX = mapGetWidth() * 16 - frogboy::SCREEN_WIDTH;
    }
}

void gameEnterDoor(DoorType door) {
    const uint8_t* doorPtr = &doorData[door * 3];

    entityInitSystem();
    critterInitSystem();
    bulletInitSystem();
    particleInitSystem();

    int16_t x = frogboy::readRom<uint8_t>(doorPtr++) * 256;
    int16_t y = frogboy::readRom<uint8_t>(doorPtr++) * 256;
    mapCurrentIndex = frogboy::readRom<uint8_t>(doorPtr++);
    playerAdd(x, y);

    wipeProgress = 0;

    mapCameraX = ents[ENT_OFFSET_PLAYER].x / 16 + 8 - frogboy::SCREEN_WIDTH / 2;
    gameUpdateCamera();
    critterUpdateAll();
}

void gameUpdatePaused() {

}

void gameUpdate() {
    if(frogboy::isPressed(frogboy::BUTTON_RESET)) {
        gameInit();
        return;
    }

    if(frogboy::isPressed(frogboy::BUTTON_PAUSE)) {
        if(!pausePressed) {
            pausePressed = true;
            gamePaused = !gamePaused;
        }
    } else {
        pausePressed = false;
    }

    if(gamePaused) {
        gameUpdatePaused();
        return;
    }

    if(wipeProgress < 32) {
        wipeProgress++;
    }
    particleUpdateAll();
    critterUpdateAll();
    bulletUpdateAll();
    playerUpdate();
    gameUpdateCamera();
}<commit_msg>Increase fully dark time during wipe transition. Do not update normal gameplay until the wipe is finished, so moving through doors is less disorienting, less chance to get hit by unseen targets<commit_after>#include <string.h>
#include "frogboy.h"
#include "game.h"
#include "entity.h"
#include "bullet.h"
#include "critter.h"
#include "player.h"
#include "particle.h"
#include "sprites_bitmap.h"
#include "map.h"

bool pausePressed;
bool gamePaused;

const char pauseMessage[] FROGBOY_ROM_DATA = "PAUSED";

uint8_t wipeProgress;
const uint8_t wipeMasks[] FROGBOY_ROM_DATA = {0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x11, 0x51, 0x55, 0xD5, 0xDD, 0xDF, 0xFF};

const uint8_t doorData[DOOR_TYPE_COUNT * 3] FROGBOY_ROM_DATA = {
    2, 2, MAP_TYPE_GRASSLAND,
    2, 2, MAP_TYPE_HOUSE,
    10, 1, MAP_TYPE_GRASSLAND,
    5, 2, MAP_TYPE_HOUSE2,
    40, 1, MAP_TYPE_GRASSLAND,
    2, 2, MAP_TYPE_HOUSE3,
    64, 2, MAP_TYPE_GRASSLAND,
};

void gameInit() {
    playerInitSystem();
    mapInitSystem();

    gameEnterDoor(DOOR_TYPE_START);

    playerUpdate();
    gamePaused = false;
    pausePressed = true;
}

void gameDraw() {
    mapDraw();
    critterDrawAll();
    playerDraw();
    bulletDrawAll();
    particleDrawAll();

    uint8_t mask = frogboy::readRom<uint8_t>(&wipeMasks[wipeProgress >> 2]);
    uint8_t* buffer = frogboy::getScreenBuffer();
    for(uint16_t i = 0; i != frogboy::SCREEN_WIDTH * (frogboy::SCREEN_HEIGHT / 8); ++i) {
        *buffer = *buffer & mask;
        buffer++;
    }

    if(gamePaused) {
        for(uint8_t i = 0; i != sizeof(pauseMessage) + 1; ++i) {
            frogboy::drawTile(32 + i * 8, 24, spritesBitmap, 0x4A, 0, false, false);
        }
        frogboy::printRomString(40, 24, spritesBitmap, pauseMessage, 1);
    }
    playerDrawHUD();
}

enum {
    CAMERA_BORDER = 6,
    CAMERA_MAX_SPEED = 4,
};

bool gameCheckOnScreen(int16_t x, int16_t y, uint8_t borderX, uint8_t borderY) {
    return x - mapCameraX < 128 + borderX
        && x - mapCameraX > -borderX
        && y - mapCameraY < 64 + borderY
        && y - mapCameraY > -borderY;
}

void gameUpdateCamera() {
    int16_t playerX = ents[ENT_OFFSET_PLAYER].x / 16 + 8;
    if(playerX < mapCameraX + frogboy::SCREEN_WIDTH / 2 - CAMERA_BORDER) {
        int16_t distance = mapCameraX + frogboy::SCREEN_WIDTH / 2 - CAMERA_BORDER - playerX;
        if(distance > CAMERA_MAX_SPEED) {
            distance = CAMERA_MAX_SPEED;
        }
        mapCameraX -= distance;
    }
    if(playerX > mapCameraX + frogboy::SCREEN_WIDTH / 2 + CAMERA_BORDER) {
        int16_t distance = playerX - (mapCameraX + frogboy::SCREEN_WIDTH / 2 + CAMERA_BORDER);
        if(distance > CAMERA_MAX_SPEED) {
            distance = CAMERA_MAX_SPEED;
        }
        mapCameraX += distance;
    }

    if(mapCameraX <= 0) {
        mapCameraX = 0;
    }
    if(mapCameraX >= mapGetWidth() * 16 - frogboy::SCREEN_WIDTH) {
        mapCameraX = mapGetWidth() * 16 - frogboy::SCREEN_WIDTH;
    }
}

void gameEnterDoor(DoorType door) {
    const uint8_t* doorPtr = &doorData[door * 3];

    entityInitSystem();
    critterInitSystem();
    bulletInitSystem();
    particleInitSystem();

    int16_t x = frogboy::readRom<uint8_t>(doorPtr++) * 256;
    int16_t y = frogboy::readRom<uint8_t>(doorPtr++) * 256;
    mapCurrentIndex = frogboy::readRom<uint8_t>(doorPtr++);
    playerAdd(x, y);

    wipeProgress = 0;

    mapCameraX = ents[ENT_OFFSET_PLAYER].x / 16 + 8 - frogboy::SCREEN_WIDTH / 2;
    gameUpdateCamera();
    critterUpdateAll();
}

void gameUpdatePaused() {

}

void gameUpdate() {
    if(frogboy::isPressed(frogboy::BUTTON_RESET)) {
        gameInit();
        return;
    }

    if(frogboy::isPressed(frogboy::BUTTON_PAUSE)) {
        if(!pausePressed) {
            pausePressed = true;
            gamePaused = !gamePaused;
        }
    } else {
        pausePressed = false;
    }

    if(gamePaused) {
        gameUpdatePaused();
        return;
    }

    if(wipeProgress < 48) {
        wipeProgress++;
    }
    if(wipeProgress >= 48) {
        particleUpdateAll();
        critterUpdateAll();
        bulletUpdateAll();
        playerUpdate();
        gameUpdateCamera();
    }
}<|endoftext|>