diff --git a/UML_Class_Diagram.png b/UML_Class_Diagram.png
index 5f40a3d..7800665 100644
Binary files a/UML_Class_Diagram.png and b/UML_Class_Diagram.png differ
diff --git a/src/Entity.cpp b/src/Entity.cpp
index a1eb423..921df1f 100644
--- a/src/Entity.cpp
+++ b/src/Entity.cpp
@@ -35,12 +35,13 @@ Entity::Entity(const pugi::xml_node& entityNode, sf::Texture* texture)
 
 void Entity::move(Orientation orientation)
 {
+    // TODO : Add speed ?
     shape.setRotation(static_cast<float>(orientation));
 
     sf::Vector2f movementVector(0,0);
     switch (orientation)
     {
-        case Orientation::Nort:
+        case Orientation::North:
             movementVector.y = -pro_maat::pixelsPerUnit;
             break;
         case Orientation::East:
@@ -64,3 +65,36 @@ const sf::RectangleShape& Entity::getShape() const
 {
     return(shape);
 }
+
+const State Entity::getState() const
+{
+    return currentState;
+}
+
+const sf::Vector2i Entity::getPosition() const
+{
+    // Safe : size is a multiple of pro_maat::pixelsPerUnit
+    uint8_t x = (shape.getPosition().x-0.5*shape.getSize().x)/pro_maat::pixelsPerUnit;
+    uint8_t y = (shape.getPosition().y-0.5*shape.getSize().y)/pro_maat::pixelsPerUnit;
+    return sf::Vector2i(x,y);
+}
+
+const std::vector<std::pair<uint8_t, uint8_t>> Entity::getOccupiedSquares() const
+{
+    std::vector<std::pair<uint8_t, uint8_t>> occupiedSquares;
+    // Safe : size is a multiple of pro_maat::pixelsPerUnit
+    uint8_t w = shape.getSize().x/pro_maat::pixelsPerUnit;
+    uint8_t h = shape.getSize().y/pro_maat::pixelsPerUnit;
+    uint8_t x = shape.getPosition().x/pro_maat::pixelsPerUnit - 0.5*w;
+    uint8_t y = shape.getPosition().y/pro_maat::pixelsPerUnit - 0.5*h;
+
+    for(int i = 0;i<w;i++)
+    {
+        for(int j = 0;j<h;j++)
+        {
+            occupiedSquares.emplace_back(x+i,y+j);
+        }
+    }
+
+    return std::move(occupiedSquares);
+}
diff --git a/src/Entity.h b/src/Entity.h
index 0f333cf..f729123 100644
--- a/src/Entity.h
+++ b/src/Entity.h
@@ -7,6 +7,7 @@
 
 #include <SFML/Graphics.hpp>
 #include <pugixml.hpp>
+#include <cinttypes>
 
 #include "Utils.h"
 
@@ -29,7 +30,7 @@ enum class State
 
 enum class Orientation
 {
-    Nort = 0,
+    North = 0,
     East = 90,
     South = 180,
     West = 270,
@@ -47,6 +48,11 @@ public:
     void update();
 
     const sf::RectangleShape& getShape() const;
+    const State getState() const;
+    const sf::Vector2i getPosition() const;
+    // Don't like it : iterates over every square every tick
+    const std::vector<std::pair<uint8_t, uint8_t>> getOccupiedSquares() const;
+
 
     /// Position of the target of the current action on the map
     sf::Vector2i target;
diff --git a/src/Level.cpp b/src/Level.cpp
index 20fbfb3..e5c4e22 100644
--- a/src/Level.cpp
+++ b/src/Level.cpp
@@ -15,6 +15,10 @@ Level::Level(const pugi::xml_document& xmlDoc, const TextureStore& textureStore)
         if(!strncmp(child.name(),"Entity",6))
         {
             entities.emplace_back(child,textures.at(child.attribute("textureId").as_int(0)).get());
+
+            // Initialize the occupied squares vector with the new entity's squares
+            std::vector<std::pair<uint8_t,uint8_t>> entitySquares = entities.rbegin()->getOccupiedSquares();
+            std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(occupiedSquares));
         }
     }
 }
@@ -29,9 +33,47 @@ void Level::render(sf::RenderWindow& renderWindow) const
 
 void Level::runStep()
 {
+    std::vector<std::pair<uint8_t,uint8_t>> newOccupiedSquares{};
+    newOccupiedSquares.reserve(occupiedSquares.size());
+
     for(Entity& entity: entities)
     {
-        // FIXME : For testing purposes
-        entity.move();
+        entity.update();
+        int heuristicSign = 0;
+        switch (entity.getState())
+        {
+            case State::Moving:
+            {
+                heuristicSign = 1;
+            }
+            case State::Fleeing:
+            {
+                if(heuristicSign == 0)
+                {
+                    heuristicSign = -1;
+                }
+                if(entity.target != entity.getPosition())
+                {
+                    entity.move(findPath(entity.getPosition(),entity.target,heuristicSign));
+                }
+                break;
+            }
+            case State::Waiting:break;
+            case State::Idle:break;
+        }
+
+        // Moves the occupied squares from the entity to the new occupied squares vector
+        std::vector<std::pair<uint8_t,uint8_t>> entitySquares = entity.getOccupiedSquares();
+        std::move(entitySquares.begin(),entitySquares.end(),std::back_inserter(newOccupiedSquares));
     }
+
+    occupiedSquares.swap(newOccupiedSquares);
+    // Sort the vector as to get O(ln(n)) complexity when searching for a square
+    std::sort(occupiedSquares.begin(),occupiedSquares.end());
+}
+
+Orientation Level::findPath(sf::Vector2i start, sf::Vector2i end, int sign)
+{
+    // TODO : A* which returns the next move
+    return Orientation::East;
 }
diff --git a/src/Level.h b/src/Level.h
index 52d9f07..681a6a6 100644
--- a/src/Level.h
+++ b/src/Level.h
@@ -28,6 +28,14 @@ private:
     std::vector<Entity> entities;
 
     const TextureStore& textures;
+
+    //
+    // Pathfinding
+    //
+
+    std::vector<std::pair<uint8_t,uint8_t>> occupiedSquares;
+
+    Orientation findPath(sf::Vector2i start, sf::Vector2i end, int sign);
 };
 
 
diff --git a/src/Utils.h b/src/Utils.h
index 4b837d3..4632cb0 100644
--- a/src/Utils.h
+++ b/src/Utils.h
@@ -13,7 +13,7 @@
 namespace pro_maat
 {
 
-static constexpr uint8_t pixelsPerUnit = 25;
+static constexpr uint8_t pixelsPerUnit = 50;
 static constexpr char levelFolder[] = "resources/";
 static constexpr char textureFolder[] = "resources/";
 static constexpr char fontFolder[] = "resources/";