diff --git a/src/main.cc b/src/main.cc
index 53f6f19..ab92e6c 100644
--- a/src/main.cc
+++ b/src/main.cc
@@ -10,6 +10,7 @@
 #include <boost/asio/streambuf.hpp>
 #include <boost/asio/use_awaitable.hpp>
 #include <boost/asio/write.hpp>
+#include <cmath>
 #include <iostream>
 #include <map>
 #include <random>
@@ -59,6 +60,7 @@ class Bot {
   asio::awaitable<void> Protocol();
   asio::awaitable<void> Join();
   asio::awaitable<bool> Move(Direction dir);
+  unsigned AStarHeuristic(std::pair<int, int> position);
   unsigned ShortestPath(int x, int y);
   asio::awaitable<void> ChooseMove();
 
@@ -177,8 +179,12 @@ asio::awaitable<bool> Bot::Move(Direction direction) {
   co_return true;
 }
 
+unsigned Bot::AStarHeuristic(std::pair<int, int> position) {
+  return std::abs(position.first - goal_x) + std::abs(position.second - goal_y);
+}
+
 unsigned Bot::ShortestPath(int x, int y) {
-  // Djikstra
+  // A*
   std::set<std::pair<int, int>> visited;
   std::multimap<unsigned, std::pair<int, int>> queue;
   auto goal = std::make_pair(goal_x, goal_y);
@@ -197,23 +203,23 @@ unsigned Bot::ShortestPath(int x, int y) {
     }
     if (!(known_map[position] & static_cast<unsigned>(Direction::LEFT)) &&
         position.first > 0) {
-      queue.emplace(distance + 1,
-                    std::make_pair(position.first - 1, position.second));
+      auto pos = std::make_pair(position.first - 1, position.second);
+      queue.emplace(distance + 1 + AStarHeuristic(pos), pos);
     }
     if (!(known_map[position] & static_cast<unsigned>(Direction::RIGHT)) &&
         position.first < field_width) {
-      queue.emplace(distance + 1,
-                    std::make_pair(position.first + 1, position.second));
+      auto pos = std::make_pair(position.first + 1, position.second);
+      queue.emplace(distance + 1 + AStarHeuristic(pos), pos);
     }
     if (!(known_map[position] & static_cast<unsigned>(Direction::UP)) &&
         position.second > 0) {
-      queue.emplace(distance + 1,
-                    std::make_pair(position.first, position.second - 1));
+      auto pos = std::make_pair(position.first, position.second - 1);
+      queue.emplace(distance + 1 + AStarHeuristic(pos), pos);
     }
     if (!(known_map[position] & static_cast<unsigned>(Direction::DOWN)) &&
         position.second < field_height) {
-      queue.emplace(distance + 1,
-                    std::make_pair(position.first, position.second + 1));
+      auto pos = std::make_pair(position.first, position.second + 1);
+      queue.emplace(distance + 1 + AStarHeuristic(pos), pos);
     }
     visited.emplace(position);
   }