Initial commitHEADmaster

4 files changed, 1074 insertions, 0 deletions

diff --git a/src/Agent.cpp b/src/Agent.cpp
new file mode 100644
index 0000000..c082b96
--- /dev/null
+++ b/src/Agent.cpp
@@ -0,0 +1,256 @@
+#include <Agent.hpp>
+
+#include <memory>
+
+#include <HyperNeat/Cppn.hpp>
+#include <HyperNeat/Population.hpp>
+
+using namespace hn;
+using namespace sf;
+using namespace std;
+
+size_t
+Agent::_currentLifetime = 1500;
+
+void
+Agent::create(Stage& stage)
+{
+    _eyeSight.resize(5);
+
+    _currCheckPoint = &stage._checkPoints.back();
+    _nextCheckPoint = &stage._checkPoints.front();
+
+    _shape.setRadius(AGENT_RAD);
+    _shape.setOrigin(AGENT_RAD, AGENT_RAD);
+    _shape.setFillColor(AGENT_COLOR);
+
+    b2CircleShape agentShape;
+    agentShape.m_radius = AGENT_RAD;
+
+    b2FixtureDef agentFixture;
+    agentFixture.shape             = &agentShape;
+    agentFixture.density           = 1.0f;
+    agentFixture.friction          = 0.3f;
+    agentFixture.filter.groupIndex = -1;
+    agentFixture.userData          = this;
+
+    b2BodyDef agentDef;
+    agentDef.type = b2_dynamicBody;
+
+    _body = stage._world.CreateBody(&agentDef);
+    _body->CreateFixture(&agentFixture);
+
+    recreate(stage, false);
+}
+
+void
+Agent::recreate(const Stage& stage, bool createNNet)
+{
+    _currCheckPoint = &stage._checkPoints.back();
+    _nextCheckPoint = &stage._checkPoints.front();
+
+    _completedCircuits = 0;
+    _lifetime          = 0;
+    _distance          = 0.0;
+    _isOnTrap          = false;
+    _isOld             = false;
+
+    auto   factoryPos = stage._factory.getPosition();
+    double factoryRot = stage._factory.getRotation() / IN_RADIANS;
+
+    _body->SetTransform({factoryPos.x, factoryPos.y}, factoryRot);
+    _body->SetLinearVelocity( { 0.0f, 0.0f });
+    _body->SetAngularVelocity(0.0f);
+
+    float pX    = _body->GetPosition().x;
+    float pY    = _body->GetPosition().y;
+    float angle = _body->GetAngle() / IN_DEGREES;
+
+    Transform tr;
+    tr.translate(pX, pY);
+    tr.rotate(angle);
+
+    float sight  = AGENT_SIGHT;
+    float iSight = AGENT_SIGHT * 0.7071f;
+
+    _eyeSight[0] = tr.transformPoint({ -sight,   0.0f});
+    _eyeSight[1] = tr.transformPoint({-iSight, iSight});
+    _eyeSight[2] = tr.transformPoint({   0.0f,  sight});
+    _eyeSight[3] = tr.transformPoint({ iSight, iSight});
+    _eyeSight[4] = tr.transformPoint({  sight,   0.0f});
+
+    if (createNNet) {
+        _organism->createNeuralNet();
+    }
+}
+
+void
+Agent::setPoints(const Stage& stage, bool passToOrg)
+{
+    if (!_organism->isOld()) {
+        return;
+    }
+
+    // Calculate fitness
+    double dist = distanceTravelled(stage);
+
+    if (passToOrg) {
+        _organism->_fitness = (dist / static_cast<double>(max(_lifetime, (size_t)1)));
+    }
+}
+
+void
+Agent::setBehavior(const Stage& stage)
+{
+    _organism->getBehavior().at(0) = _body->GetPosition().x;
+    _organism->getBehavior().at(1) = _body->GetPosition().y;
+    _organism->getBehavior().at(2) = (distanceTravelled(stage) / static_cast<double>(max(_lifetime, (size_t)1)));
+}
+
+void
+Agent::update(const Stage& stage, bool withNS)
+{
+    if (withNS) {
+        if (_organism->getLifetime() >= (_currentLifetime - 1)) {
+            if (!_isOld) {
+                _isOld = true;
+            }
+        } else if (_organism->getLifetime() < (_currentLifetime - 1)) {
+            if (_isOld) {
+                _isOld = false;
+            }
+        }
+    }
+
+    ++_lifetime;
+
+    if (_isOnTrap) {
+        return;
+    }
+
+    float pX    = _body->GetPosition().x;
+    float pY    = _body->GetPosition().y;
+    float angle = _body->GetAngle() / IN_DEGREES;
+
+    Transform tr, rot;
+    tr.translate(pX, pY);
+    tr.rotate(angle);
+    rot.rotate(angle);
+
+    float sight  = AGENT_SIGHT;
+    float iSight = AGENT_SIGHT * 0.7071f;
+
+    _eyeSight[0] = tr.transformPoint({ -sight,   0.0f});
+    _eyeSight[1] = tr.transformPoint({-iSight, iSight});
+    _eyeSight[2] = tr.transformPoint({   0.0f,  sight});
+    _eyeSight[3] = tr.transformPoint({ iSight, iSight});
+    _eyeSight[4] = tr.transformPoint({  sight,   0.0f});
+
+    for (size_t i = 0; i < 5; ++i) {
+        RayCastCallback callback;
+        b2Vec2 b2EyeSight = { _eyeSight[i].x, _eyeSight[i].y };
+        stage._world.RayCast(&callback, _body->GetPosition(), b2EyeSight);
+        _organism->_neuralNet->inputAt(i) = callback._fraction;
+    }
+
+    _organism->_neuralNet->cycle();
+
+    Vector2f velocity
+        = rot.transformPoint({ 0.0f, static_cast<float>(_organism->_neuralNet->outputAt(1)) * AGENT_SPEED });
+    float rotation
+        = (((_organism->_neuralNet->outputAt(0) + _organism->_neuralNet->outputAt(2)) * 2.0f - 1.0f) / IN_RADIANS) *
+        AGENT_SPEED;
+
+    _body->SetLinearVelocity({velocity.x, velocity.y});
+    _body->SetAngularVelocity(rotation);
+}
+
+void
+Agent::drawOn(sf::RenderWindow& surface)
+{
+    float pX = _body->GetPosition().x;
+    float pY = _body->GetPosition().y;
+
+    _shape.setPosition(pX, pY);
+
+    VertexArray eyeSightLines(Lines);
+
+    for (auto & i : _eyeSight) {
+        eyeSightLines.append({{pX, pY}, SIGHT_COLOR});
+        eyeSightLines.append({       i, SIGHT_COLOR});
+    }
+
+    surface.draw(_shape);
+    surface.draw(eyeSightLines);
+}
+
+void
+Agent::incrementCheckPoint(Stage& stage)
+{
+    _currCheckPoint = _nextCheckPoint;
+
+    if (_currCheckPoint == &stage._checkPoints.back()) {
+        _nextCheckPoint = &stage._checkPoints.front();
+    } else {
+        ++_nextCheckPoint;
+    }
+
+    if (_currCheckPoint == &stage._checkPoints.back()) {
+        ++_completedCircuits;
+    }
+}
+
+double
+Agent::distanceTravelled(const Stage& stage)
+{
+    double pCurr = _currCheckPoint->_value;
+    double pLoop = stage._checkPoints.back()._value;
+
+    if (_currCheckPoint == &stage._checkPoints.back()) {
+        pCurr -= pLoop;
+    }
+
+    double dNext = distanceToCheckpoint(*_nextCheckPoint);
+    double sNext = _nextCheckPoint->_segment;
+
+    if (dNext > sNext) {
+        _distance = pCurr + (pLoop * _completedCircuits);
+    } else {
+        double pNext = _nextCheckPoint->_value;
+        double f1    = dNext / sNext;
+        double f2    = 1.0 - f1;
+        _distance    = (pCurr * f1) + (pNext * f2) + (pLoop * _completedCircuits);
+    }
+
+    if (_distance < 1.0) {
+        _distance = 1.0;
+    }
+
+    return _distance;
+}
+
+float
+Agent::distanceToCheckpoint(const Stage::CheckPoint& cp) const
+{
+    b2Vec2 p = _body->GetPosition();
+    b2Vec2 v = {cp._line[0].position.x, cp._line[0].position.y};
+    b2Vec2 w = {cp._line[1].position.x, cp._line[1].position.y};
+
+    float l2 = (w - v).LengthSquared();
+
+    if (l2 == 0.0f) {
+        return dist(p, v);
+    }
+
+    float t = dotP(p - v, w - v) / l2;
+
+    if (t < 0.0f) {
+        return dist(p, v);
+    } else if (t > 1.0f) {
+        return dist(p, w);
+    }
+
+    b2Vec2 projection = v + t * (w - v);
+
+    return dist(p, projection);
+}
diff --git a/src/App.cpp b/src/App.cpp
new file mode 100644
index 0000000..c97061e
--- /dev/null
+++ b/src/App.cpp
@@ -0,0 +1,538 @@
+#include <App.hpp>
+#include <HyperNeat/Cppn.hpp>
+#include <tbb/parallel_for.h>
+#include <HyperNeat/Utils/Thread.hpp>
+#include <HyperNeat/Utils/LoadFile.hpp>
+#include <HyperNeat/Utils/SaveFile.hpp>
+
+using namespace hn;
+using namespace sf;
+using namespace tbb;
+using namespace std;
+
+int
+App::startup()
+{
+    ifstream paramsFile("parameters");
+    ifstream popFile("current.population");
+
+    if (!paramsFile) {
+        return EXIT_FAILURE;
+    }
+
+    cout << "Type hours to run sim.: ";
+    cin  >> _secondsToRun;
+    _secondsToRun *= (60 * 60);
+
+    bool popLoaded = false;
+
+    if (popFile) {
+        popLoaded = true;
+
+        LoadFile lFile(popFile);
+        lFile.loadPopulation(_population);
+
+        _popPrms         = _population.getPopulationPrms();
+        _doNoveltySearch = _population.isNoveltyMetricSet();
+    }
+
+    auto until = numeric_limits<streamsize>::max();
+
+    while (!paramsFile.eof()) {
+        paramsFile.ignore(until, '<');
+
+        string command;
+        getline(paramsFile, command, '>');
+
+        if (!popLoaded && command == "POPULATION_PARAMETERS") {
+            paramsFile.ignore(until, '<');
+
+            while (paramsFile.peek() != '/') {
+                string subCommand;
+                getline(paramsFile, subCommand, '>');
+
+                if (subCommand == "populationSize") {
+                    paramsFile >> _popPrms._popSize;
+                } else if (subCommand == "seed") {
+                    paramsFile >> _popPrms._seed;
+                } else if (subCommand == "weightRange") {
+                    paramsFile >> _popPrms._weightRange;
+                } else if (subCommand == "c1Disjoint") {
+                    paramsFile >> _popPrms._c1Disjoint;
+                } else if (subCommand == "c3WeightDifference") {
+                    paramsFile >> _popPrms._c3WeightDifference;
+                } else if (subCommand == "populationSize") {
+                    paramsFile >> _popPrms._popSize;
+                } else if (subCommand == "initialDistanceThreshold") {
+                    paramsFile >> _popPrms._initialDistanceThreshold;
+                } else if (subCommand == "distanceThresholdShift") {
+                    paramsFile >> _popPrms._distanceThresholdShift;
+                } else if (subCommand == "sexualReproductionRate") {
+                    paramsFile >> _popPrms._sexualReproductionRate;
+                } else if (subCommand == "weightMutationRate") {
+                    paramsFile >> _popPrms._weightMutationRate;
+                } else if (subCommand == "weightDeviation") {
+                    paramsFile >> _popPrms._weightDeviation;
+                } else if (subCommand == "interspeciesMatingRate") {
+                    paramsFile >> _popPrms._interspeciesMatingRate;
+                } else if (subCommand == "geneDisablingRatio") {
+                    paramsFile >> _popPrms._geneDisablingRatio;
+                } else if (subCommand == "linkMutationRate") {
+                    paramsFile >> _popPrms._linkMutationRate;
+                } else if (subCommand == "nodeMutationRate") {
+                    paramsFile >> _popPrms._nodeMutationRate;
+                } else if (subCommand == "targetSpeciesCount") {
+                    paramsFile >> _popPrms._targetSpeciesCount;
+                } else if (subCommand == "eligibilityRatio") {
+                    paramsFile >> _popPrms._eligibilityRatio;
+                } else if (subCommand == "minimumLifetime") {
+                    paramsFile >> _popPrms._minimumLifetime;
+                } else if (subCommand == "replBeforeReorganization") {
+                    paramsFile >> _popPrms._replBeforeReorganization;
+                }
+
+                paramsFile.ignore(until, '<');
+            }
+        } else if (command == "NEURAL_NET_PARAMETERS") {
+            paramsFile.ignore(until, '<');
+
+            while (paramsFile.peek() != '/') {
+                string subCommand;
+                getline(paramsFile, subCommand, '>');
+
+                if (subCommand == "testGridLevel") {
+                    paramsFile >> _nnPrms._testGridLevel;
+                } else if (subCommand == "maxQuadTreeLevel") {
+                    paramsFile >> _nnPrms._maxQuadTreeLevel;
+                } else if (subCommand == "minQuadTreeLevel") {
+                    paramsFile >> _nnPrms._minQuadTreeLevel;
+                } else if (subCommand == "bandPruningThreshold") {
+                    paramsFile >> _nnPrms._bandPruningThreshold;
+                } else if (subCommand == "varianceThreshold") {
+                    paramsFile >> _nnPrms._varianceThreshold;
+                } else if (subCommand == "divisionThreshold") {
+                    paramsFile >> _nnPrms._divisionThreshold;
+                } else if (subCommand == "searchIterations") {
+                    paramsFile >> _nnPrms._iterations;
+                }
+
+                paramsFile.ignore(until, '<');
+            }
+        } else if (command == "NOVELTY_SEARCH_PARAMETERS") {
+            paramsFile.ignore(until, '<');
+
+            while (paramsFile.peek() != '/') {
+                string subCommand;
+                getline(paramsFile, subCommand, '>');
+
+                if (subCommand == "doNoveltySearch") {
+                    paramsFile >> _doNoveltySearch;
+                } else if (subCommand == "noveltyThreshold") {
+                    paramsFile >> _noveltyPrms._noveltyThreshold;
+                } else if (subCommand == "referenceOrganisms") {
+                    paramsFile >> _noveltyPrms._referenceOrganisms;
+                }
+
+                _noveltyPrms._characterizationSize = 3;
+
+                paramsFile.ignore(until, '<');
+            }
+        } else if (command == "STAGE_PARAMETERS") {
+            paramsFile.ignore(until, '<');
+
+            while (paramsFile.peek() != '/') {
+                string subCommand;
+                getline(paramsFile, subCommand, '>');
+
+                if (subCommand == "file") {
+                    string fileName;
+                    paramsFile >> fileName;
+
+                    if (_stage.load(fileName) == EXIT_FAILURE) {
+                        return EXIT_FAILURE;
+                    }
+                } else if (subCommand == "draw") {
+                    paramsFile >> _draw;
+                }
+
+                paramsFile.ignore(until, '<');
+            }
+        }
+    }
+
+    if (!_draw) {
+        _isVSyncEnabled = false;
+        _isPaused       = false;
+    }
+
+    _nnPrms._inputMap = {
+        { -1.0,  -0.5 },
+        { -0.7,   0.2 },
+        {  0.0,   0.5 },
+        {  0.7,   0.2 },
+        {  1.0,  -0.5 }
+    };
+
+    _nnPrms._outputMap = {
+        { -0.5, -0.5 },
+        {  0.0,  0.0 },
+        {  0.5, -0.5 }
+    };
+
+    if (!popLoaded) {
+        _population.create(_popPrms, _nnPrms);
+    }
+
+    if (_doNoveltySearch && !_population.isNoveltyMetricSet()) {
+        _population.setNoveltyMetric(_noveltyPrms);
+    } else if (!_doNoveltySearch && _population.isNoveltyMetricSet()) {
+        _population.clearNoveltyMetric();
+    }
+
+    _agents.resize(_popPrms._popSize, Agent());
+
+    for (size_t i = 0; i < _agents.size(); ++i) {
+        _agents[i]._organism = &_population.getOrganism(i);
+        _agents[i].create(_stage);
+    }
+
+    // Agent::_currentLifetime = (_stage._checkPoints.back()._value / 1.75);
+
+    _champ.setFillColor(Color::Transparent);
+    _champ.setOutlineColor(CHAMP_COLOR);
+    _champ.setOutlineThickness(2);
+    _champ.setRadius(AGENT_RAD + 2);
+    _champ.setOrigin(AGENT_RAD + 2, AGENT_RAD + 2);
+
+    _selected = _champ;
+    _selected.setOutlineColor(SELECTED_COLOR);
+
+    // Print NoveltyMarks
+    if (_population.isNoveltyMetricSet()) {
+        for (auto& i : _population.getNoveltyMetric().getArchive()) {
+            _noveltyMarks.emplace_back();
+            _noveltyMarks.back().setFillColor({ 255, 255, 0, 10 });
+            _noveltyMarks.back().setRadius(AGENT_RAD + 2);
+            _noveltyMarks.back().setOrigin(AGENT_RAD + 2, AGENT_RAD + 2);
+            _noveltyMarks.back().setPosition(i[0], i[1]);
+        }
+    }
+
+    // Prepare text
+    if (!_font.loadFromFile("C:/Windows/Fonts/font_1.ttf")) {
+        return EXIT_FAILURE;
+    }
+
+    _updates.setFont(_font);
+    _champFitness.setFont(_font);
+    _avergFitness.setFont(_font);
+
+    _updates.setCharacterSize(11);
+    _champFitness.setCharacterSize(11);
+    _avergFitness.setCharacterSize(11);
+
+    _updates.setPosition(5, 5);
+    _champFitness.setPosition(5, 18);
+    _avergFitness.setPosition(5, 31);
+
+    _updates.setColor(TEXT_COLOR);
+    _champFitness.setColor(TEXT_COLOR);
+    _avergFitness.setColor(TEXT_COLOR);
+
+    return EXIT_SUCCESS;
+}
+
+int
+App::execute()
+{
+    if (!_draw) {
+        hn::Thread runner([&]() {
+            while (_runningWOGfx) {
+                update();
+            }
+        });
+
+        cin.get();
+        cin.get();
+
+        _runningWOGfx = false;
+        runner.join();
+
+        return EXIT_SUCCESS;
+    }
+
+    _window.create(VIDEO_MODE, WIN_TITLE, WIN_STYLE, WIN_SETTINGS);
+
+    _clock.restart();
+    _globalClock.restart();
+
+    while (_window.isOpen()) {
+        if (!_isPaused) {
+            update();
+        }
+
+        Event event;
+
+        while (_window.pollEvent(event)) {
+            if (event.type == Event::Closed) {
+                _cppnEx.shutdown();
+                _window.close();
+            } else if (event.type == Event::KeyPressed) {
+                if (event.key.code == Keyboard::Space) {
+                    _isPaused = !_isPaused;
+                } else if (event.key.code == Keyboard::V) {
+                    _isVSyncEnabled = !_isVSyncEnabled;
+
+                    if (_isVSyncEnabled) {
+                        _window.setVerticalSyncEnabled(true);
+                    } else {
+                        _window.setVerticalSyncEnabled(false);
+                    }
+                } else if (event.key.code == Keyboard::L) {
+                    if (_selectedIdx != -1) {
+                        if (_population.isOrganismLocked(_selectedIdx)) {
+                            _population.unlockOrganism(_selectedIdx);
+                        } else {
+                            _population.lockOrganism(_selectedIdx);
+                        }
+                    }
+                } else if (event.key.code == Keyboard::F) {
+                    if (_selectedIdx != -1) {
+                        if (_population.isOrganismFrozen(_selectedIdx)) {
+                            _population.unfreezeOrganism(_selectedIdx);
+                        } else {
+                            _population.freezeOrganism(_selectedIdx);
+                        }
+                    }
+                } else if (event.key.code == Keyboard::C) {
+                    if (_selectedIdx != -1) {
+                        auto& genome = _population.getOrganism(_selectedIdx).getGenome();
+                        _cppnEx.run(genome, &_nnPrms);
+                    }
+                }
+            } else if (event.type == Event::MouseButtonPressed && event.mouseButton.button == Mouse::Left) {
+                float cX = static_cast<float>(Mouse::getPosition(_window).x);
+                float cY = static_cast<float>(Mouse::getPosition(_window).y);
+
+                b2AABB aabb;
+                aabb.lowerBound = { cX - 0.5f, cY - 0.5f };
+                aabb.upperBound = { cX + 0.5f, cY + 0.5f };
+
+                QueryCallback qCallback;
+
+                _stage._world.QueryAABB(&qCallback, aabb);
+
+                if (qCallback._fixture) {
+                    if (qCallback._fixture->GetFilterData().groupIndex == -1) {
+                        Agent* agent    = static_cast<Agent*>(qCallback._fixture->GetUserData());
+                        size_t agentIdx = 0;
+
+                        while (&_agents[agentIdx] != agent) {
+                            ++agentIdx;
+                        }
+
+                        _selectedIdx = agentIdx;
+
+                        size_t oldOrg      = 0;
+                        double averageDist = 0.0;
+
+                        for (auto& i : _agents) {
+                            if (i._organism->isOld()) {
+                                ++oldOrg;
+                                averageDist += i._distance;
+                            }
+                        }
+
+                        averageDist /= static_cast<double>(oldOrg);
+
+                        cout.setf(ios::boolalpha);
+                        cout << endl;
+                        cout << "completedCircuits : " << _agents[agentIdx]._completedCircuits << endl;
+                        cout << "lifetime          : " << _agents[agentIdx]._lifetime << endl;
+                        cout << "distance          : " << _agents[agentIdx]._distance << endl;
+                        cout << "isOnTrap          : " << _agents[agentIdx]._isOnTrap << endl;
+                        cout << "isOld             : " << _agents[agentIdx]._organism->isOld() << endl;
+                        // cout << "noveltyScore      : " << _agents[agentIdx]._organism->getBehavior().getNoveltyScore() << endl;
+                        // cout << "criteriaReached   : " << _agents[agentIdx]._organism->getBehavior()._criteriaReached << endl;
+                        cout << "averageDist       : " << averageDist << endl;
+                        cout << endl;
+                    } else {
+                        _selectedIdx = -1;
+                    }
+                } else {
+                    _selectedIdx = -1;
+                }
+            }
+        }
+
+        draw();
+    }
+
+    return EXIT_SUCCESS;
+}
+
+void
+App::update()
+{
+    parallel_for(size_t(0), _agents.size(), size_t(1), [&](size_t i) {
+        if (!_agents[i]._organism->isBeingGenerated()) {
+            _agents[i].update(_stage, _doNoveltySearch);
+        }
+    });
+
+    _population.update([&]() {
+        if (!_doNoveltySearch) {
+            for (auto& i : _agents) {
+                i.setPoints(_stage, true);
+            }
+        } else {
+            for (auto& i : _agents) {
+                i.setBehavior(_stage);
+            }
+        }
+    }, [&]() {
+        _agents[_population.getLastReplacement()->getIndex()].recreate(_stage, true);
+    });
+
+    if (_doNoveltySearch) {
+        size_t maxCircuits = 0;
+
+        for (auto& i : _agents) {
+            maxCircuits = max(maxCircuits, i._completedCircuits);
+        }
+
+        if (maxCircuits != _completedCircuits) {
+            _completedCircuits = maxCircuits;
+            size_t newLifetime = (_stage._checkPoints.back()._value / 1.75) * (_completedCircuits + 1);
+
+            _population.setMinimumLifetime(newLifetime);
+            Agent::_currentLifetime = newLifetime;
+        }
+    }
+
+    for (auto& i : _stage._traps) {
+        TrapCallback trapCallback;
+        _stage._world.QueryAABB(&trapCallback, i);
+    }
+
+    if (_doNoveltySearch) {
+        auto& archive = _population.getNoveltyMetric().getArchive();
+
+        if (archive.size() > _noveltyMarks.size()) {
+            for (size_t i = _noveltyMarks.size(); i < archive.size(); ++i) {
+                _noveltyMarks.emplace_back();
+                _noveltyMarks.back().setFillColor({ 255, 255, 0, 10 });
+                _noveltyMarks.back().setRadius(AGENT_RAD + 2);
+                _noveltyMarks.back().setOrigin(AGENT_RAD + 2, AGENT_RAD + 2);
+                _noveltyMarks.back().setPosition(archive[i][0], archive[i][1]);
+            }
+        }
+    }
+
+    _stage.update();
+
+    if (_population.getUpdates() % 10000 == 0) {
+        ofstream popFile("current.population");
+        SaveFile sFile(popFile);
+        sFile.savePopulation(_population, true);
+
+        using chrono::system_clock;
+        system_clock::time_point today = system_clock::now();
+
+        time_t tt = system_clock::to_time_t(today);
+        cout << "> Saved on       : " << ctime(&tt);
+        cout << "> Champ Fitness  : " << _population.getChampion()._fitness << endl;
+        cout << "> Comp. Circuits : " << _completedCircuits << endl;
+
+        if (_population.isNoveltyMetricSet()) {
+            cout << "> Archive Size   : " << _population.getNoveltyMetric().getArchive().size() << endl << endl;
+        } else {
+            cout << endl;
+        }
+    }
+
+    if (_globalClock.getElapsedTime().asSeconds() > _secondsToRun) {
+        _window.close();
+        _runningWOGfx = false;
+    }
+}
+
+void
+App::draw()
+{
+    if (!_isVSyncEnabled) {
+        if (_clock.getElapsedTime().asSeconds() < 1.0f / 60.0f) {
+            return;
+        }
+
+        _clock.restart();
+    }
+
+    _window.clear(BG_COLOR);
+
+    _stage.drawOn(_window);
+
+    for (auto& i : _noveltyMarks) {
+        _window.draw(i);
+    }
+
+    for (size_t i = 0, end = _agents.size(); i < end; ++i) {
+        if (!_population.isOrganismBeingGenerated(i)) {
+            _agents[i].drawOn(_window);
+        }
+    }
+
+    // Highlight champ
+    double  champFtn      = _population.getChampion()._fitness;
+    ssize_t champIdx      = _population.getChampion().getIndex();
+    double averageFitness = _population.getAverageFitness();
+
+    if (champIdx != -1) {
+        auto cPos = _agents[champIdx]._body->GetPosition();
+        _champ.setPosition(cPos.x, cPos.y);
+        _window.draw(_champ);
+    }
+
+    // Highlight selected
+    if (_selectedIdx != -1) {
+        auto sPos = _agents[_selectedIdx]._body->GetPosition();
+        _selected.setPosition(sPos.x, sPos.y);
+        _window.draw(_selected);
+    }
+
+    // Print text
+    _updates.setString     ("Simulation updates : " + toString(_population.getUpdates()));
+    _champFitness.setString("Champ's fitness    : " + toString(champFtn));
+    _avergFitness.setString("Average fitness    : " + toString(averageFitness));
+
+    _window.draw(_updates);
+    _window.draw(_champFitness);
+    _window.draw(_avergFitness);
+
+    _window.display();
+}
+
+void
+App::shutdown()
+{
+    _population.shutdown(true);
+
+    ofstream popFile("current.population");
+    SaveFile sFile(popFile);
+    sFile.savePopulation(_population);
+
+    cout << "SHUTTING DOWN!" << endl;
+}
+
+int
+main()
+{
+    App app;
+
+    if (app.startup() == EXIT_FAILURE || app.execute() == EXIT_FAILURE) {
+        return EXIT_FAILURE;
+    } else {
+        app.shutdown();
+        return EXIT_SUCCESS;
+    }
+}
diff --git a/src/NR_Utils.cpp b/src/NR_Utils.cpp
new file mode 100644
index 0000000..2768dad
--- /dev/null
+++ b/src/NR_Utils.cpp
@@ -0,0 +1,88 @@
+#include <NR_Utils.hpp>
+#include <Agent.hpp>
+#include <Stage.hpp>
+
+using namespace std;
+
+ssize_t
+pMod(ssize_t n, ssize_t d)
+{
+    return (n % d + d) % d;
+}
+
+float
+dist(const b2Vec2& a, const b2Vec2 b)
+{
+    auto x = a.x - b.x;
+    auto y = a.y - b.y;
+
+    return sqrt(x * x + y * y);
+}
+
+float
+dotP(const b2Vec2& a, const b2Vec2 b)
+{
+    return a.x * b.x + a.y * b.y;
+}
+
+float32
+RayCastCallback::ReportFixture(b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction)
+{
+    if (fixture->GetFilterData().groupIndex == -1 || fixture->IsSensor()) {
+        return -1.0f;
+    }
+
+    _fixture  = fixture;
+    _fraction = 1.0f - fraction;
+
+    return fraction;
+}
+
+bool
+QueryCallback::ReportFixture(b2Fixture* fixture)
+{
+    _fixture = fixture;
+
+    return false;
+}
+
+bool
+TrapCallback::ReportFixture(b2Fixture* fixture)
+{
+    if (fixture->GetBody()->GetType() != b2_dynamicBody) {
+        return true;
+    }
+
+    Agent& agent    = *static_cast<Agent*>(fixture->GetUserData());
+    agent._isOnTrap = true;
+    agent._body->SetLinearVelocity({ 0.0f, 0.0f });
+    agent._body->SetAngularVelocity(0.0f);
+
+    return true;
+}
+
+void
+ContactListener::BeginContact(b2Contact* contact)
+{
+    b2Fixture* fixA = contact->GetFixtureA();
+    b2Fixture* fixB = contact->GetFixtureB();
+
+    if (!fixA->IsSensor() && !fixB->IsSensor()) {
+        return;
+    }
+
+    Agent*     agent = nullptr;
+    b2Fixture* check = nullptr;
+
+    if (fixA->IsSensor()) {
+        check = fixA;
+        agent = static_cast<Agent*>(fixB->GetUserData());
+    } else {
+        check = fixB;
+        agent = static_cast<Agent*>(fixA->GetUserData());
+    }
+
+    if (check->GetUserData() == agent->_nextCheckPoint) {
+        agent->incrementCheckPoint(*_stage);
+    }
+}
diff --git a/src/Stage.cpp b/src/Stage.cpp
new file mode 100644
index 0000000..b9fa67a
--- /dev/null
+++ b/src/Stage.cpp
@@ -0,0 +1,192 @@
+#include <Stage.hpp>
+
+using namespace sf;
+using namespace std;
+
+int
+Stage::load(const std::string& fileName)
+{
+    ifstream stageFile(fileName);
+
+    if (!stageFile) {
+        return EXIT_FAILURE;
+    }
+
+    auto until = numeric_limits<streamsize>::max();
+
+    while (!stageFile.eof()) {
+        stageFile.ignore(until, '<');
+
+        string command;
+        getline(stageFile, command, '>');
+
+        if (command == "WALL") {
+            _walls.emplace_back(LinesStrip);
+            hn::Vector<b2Vec2> vertices;
+
+            stageFile.ignore(until, '<');
+
+            while (stageFile.peek() != '/') {
+                Vector2f vtxPos;
+
+                stageFile >> vtxPos.x;
+                stageFile.ignore(until, ',');
+                stageFile >> vtxPos.y;
+                stageFile.ignore(until, '<');
+
+                _walls.back().append({ vtxPos, WALL_COLOR });
+                vertices.emplace_back(vtxPos.x, vtxPos.y);
+            }
+
+            b2ChainShape shape;
+            shape.CreateChain(vertices.data(), vertices.size());
+
+            b2BodyDef bodyDef;
+            _wallBodies.emplace_back(_world.CreateBody(&bodyDef));
+            _wallBodies.back()->CreateFixture(&shape, 0.0f);
+        } else if (command == "FACTORY") {
+            Vector2f factPos;
+            double   factRot = 0.0;
+
+            stageFile.ignore(until, '<');
+            stageFile >> factPos.x;
+            stageFile.ignore(until, ',');
+            stageFile >> factPos.y;
+            stageFile.ignore(until, ',');
+            stageFile >> factRot;
+            stageFile.ignore(until, '<');
+
+            _factory.setPosition(factPos);
+            _factory.setRotation(factRot);
+            _factory.setOrigin(AGENT_RAD + 2, AGENT_RAD + 2);
+            _factory.setFillColor(Color::Black);
+            _factory.setOutlineColor(FACTORY_COLOR);
+            _factory.setOutlineThickness(2.0f);
+        } else if (command == "CHECKPOINT") {
+            stageFile.ignore(until, '<');
+
+            while (stageFile.peek() != '/') {
+                _checkPoints.emplace_back();
+
+                Vector2f vtx1Pos;
+                Vector2f vtx2Pos;
+                b2Vec2   shVx[2];
+
+                stageFile >> vtx1Pos.x;
+                shVx[0].x  = vtx1Pos.x;
+                stageFile.ignore(until, ',');
+                stageFile >> vtx1Pos.y;
+                shVx[0].y  = vtx1Pos.y;
+                stageFile.ignore(until, ',');
+                stageFile >> vtx2Pos.x;
+                shVx[1].x  = vtx2Pos.x;
+                stageFile.ignore(until, ',');
+                stageFile >> vtx2Pos.y;
+                shVx[1].y  = vtx2Pos.y;
+                stageFile.ignore(until, ',');
+                stageFile >> _checkPoints.back()._value;
+                stageFile.ignore(until, '<');
+
+                _checkPoints.back()._line.append({ vtx1Pos, CHECKPNT_COLOR });
+                _checkPoints.back()._line.append({ vtx2Pos, CHECKPNT_COLOR });
+
+                _checkPoints.front().calculateSegment(_checkPoints.back());
+
+                b2ChainShape cpShape;
+                cpShape.CreateChain(shVx, 2);
+
+                b2FixtureDef cpFixture;
+                cpFixture.shape    = &cpShape;
+                cpFixture.isSensor = true;
+
+                b2BodyDef cpDef;
+                _checkPoints.back()._body = _world.CreateBody(&cpDef);
+                _checkPoints.back()._body->CreateFixture(&cpFixture);
+
+                if (_checkPoints.size() > 1) {
+                    auto& prev = _checkPoints[_checkPoints.size() - 2];
+
+                    _checkPoints.back().calculateSegment(prev);
+                }
+            }
+
+            for (auto & i : _checkPoints) {
+                i._body->GetFixtureList()->SetUserData(&i);
+            }
+        } else if (command == "TRAP") {
+            stageFile.ignore(until, '<');
+
+            while (stageFile.peek() != '/') {
+                sf::Vector2f v1;
+                sf::Vector2f v2;
+
+                stageFile >> v1.x;
+                stageFile.ignore(until, ',');
+                stageFile >> v1.y;
+                stageFile.ignore(until, ',');
+                stageFile >> v2.x;
+                stageFile.ignore(until, ',');
+                stageFile >> v2.y;
+                stageFile.ignore(until, '<');
+
+                _trapRects.emplace_back(sf::Vector2f(v2.x - v1.x, v2.y - v1.y));
+                _trapRects.back().setPosition(v1);
+                _trapRects.back().setFillColor({ 128, 32, 32, 128 });
+
+                _traps.emplace_back();
+                _traps.back().lowerBound.Set(v1.x, v1.y);
+                _traps.back().upperBound.Set(v2.x, v2.y);
+            }
+        }
+    }
+
+    _world.SetContactListener(&_listener);
+    _listener._stage = this;
+
+    return EXIT_SUCCESS;
+}
+
+void
+Stage::update()
+{
+    _world.Step(_TIME_STEP, _VEL_ITERS, _POS_ITERS);
+}
+
+void
+Stage::drawOn(RenderWindow& surface) const
+{
+    for (auto& i : _trapRects) {
+        surface.draw(i);
+    }
+
+    for (auto& i : _walls) {
+        surface.draw(i);
+    }
+
+    surface.draw(_factory);
+
+    for (auto& i : _checkPoints) {
+        surface.draw(i._line);
+    }
+}
+
+void
+Stage::CheckPoint::calculateSegment(const CheckPoint& prev)
+{
+    float32 a0X = prev._line[0].position.x;
+    float32 a0Y = prev._line[0].position.y;
+    float32 a1X = prev._line[1].position.x;
+    float32 a1Y = prev._line[1].position.y;
+
+    float32 b0X = _line[0].position.x;
+    float32 b0Y = _line[0].position.y;
+    float32 b1X = _line[1].position.x;
+    float32 b1Y = _line[1].position.y;
+
+    float32 dA0B0 = dist( { a0X, a0Y }, { b0X, b0Y });
+    float32 dA0B1 = dist( { a0X, a0Y }, { b1X, b1Y });
+    float32 dA1B0 = dist( { a1X, a1Y }, { b0X, b0Y });
+    float32 dA1B1 = dist( { a1X, a1Y }, { b1X, b1Y });
+
+    _segment = min(min(dA0B0, dA0B1), min(dA1B0, dA1B1));
+}