From ce36d9a00c4b77c64a416c3e305a3c9353d016f3 Mon Sep 17 00:00:00 2001
From: devZoGok <ekokekoa@yandex.com>
Date: Sat, 6 Aug 2022 17:59:44 +0300
Subject: [PATCH] implemented Djikstra algorithm

---
 pathfinder.cpp     | 49 +++++++++++++++++++++++++++++++++++++++-------
 pathfinder.h       |  4 +++-
 pathfinderTest.cpp | 29 +++++++++++++++------------
 3 files changed, 61 insertions(+), 21 deletions(-)

diff --git a/pathfinder.cpp b/pathfinder.cpp
index fac7242..261ec96 100644
--- a/pathfinder.cpp
+++ b/pathfinder.cpp
@@ -1,7 +1,10 @@
+#include <algorithm>
+
 #include "pathfinder.h"
 
 namespace battleship{
 		using namespace std;
+		using namespace vb01;
 
 		static Pathfinder *pathfinder = nullptr;
 
@@ -15,14 +18,46 @@ namespace battleship{
 		Pathfinder::Pathfinder(){
 		}
 
-		vector<int> Pathfinder::findPath(int **weights, int size, int source, int sink){
-				int pathWeights[size];
-				pathWeights[0] = 0;
+		vector<int> Pathfinder::findPath(s32 **weights, int size, int source, int dest){
+				s32 distances[size];
+				vector<int> paths[size];
+				paths[source].push_back(source);
 
-				for(int i = 1; i < size; i++)
-						pathWeights[i] = -1;
+				for(int i = 0; i < size; i++)
+						distances[i] = (i == source ? 0 : 999);
 
-				vector<int> path;
-				return path;
+				vector<int> checkedNodes;
+
+				while(find(checkedNodes.begin(), checkedNodes.end(), dest) == checkedNodes.end()){
+					bool initVertStrichSet = false;
+					int vertStrich;
+
+					for(int i = 0; i < size; i++){
+						bool isChecked = find(checkedNodes.begin(), checkedNodes.end(), i) != checkedNodes.end();
+
+						if(!isChecked){
+							if(!initVertStrichSet){
+								vertStrich = i;
+								initVertStrichSet = true;
+							}
+							if(initVertStrichSet && (distances[vertStrich] > distances[i]))
+								vertStrich = i;
+						}
+					}
+
+					checkedNodes.push_back(vertStrich);
+
+					for(int i = 0; i < size; i++){
+						bool isChecked = find(checkedNodes.begin(), checkedNodes.end(), i) != checkedNodes.end();
+
+						if(!isChecked && (distances[vertStrich] + weights[vertStrich][i] < distances[i])){
+							distances[i] = distances[vertStrich] + weights[vertStrich][i];
+							paths[i] = paths[vertStrich];
+							paths[i].push_back(i);
+						}
+					}
+				}
+
+				return paths[dest];
 		}
 }
diff --git a/pathfinder.h b/pathfinder.h
index cc3d79c..e00ff94 100644
--- a/pathfinder.h
+++ b/pathfinder.h
@@ -3,11 +3,13 @@
 
 #include <vector>
 
+#include <util.h>
+
 namespace battleship{
 		class Pathfinder{
 				public:
 						static Pathfinder* getSingleton();
-						std::vector<int> findPath(int**, int, int, int);
+						std::vector<int> findPath(vb01::s32**, int, int, int);
 				private:
 						Pathfinder();
 		};
diff --git a/pathfinderTest.cpp b/pathfinderTest.cpp
index 979167a..f45f8ab 100644
--- a/pathfinderTest.cpp
+++ b/pathfinderTest.cpp
@@ -1,38 +1,41 @@
 #include "pathfinderTest.h"
 #include "pathfinder.h"
 
+#include <util.h>
+
 #include <vector>
 
 namespace battleship{
 		using namespace std;
+		using namespace vb01;
 
 		void PathfinderTest::testFindPath(){
 				int size = 7;
-				int w[size][size] = {
-						{0, 2, 4, -1, -1, -1, -1},
-						{-1, 0, 1, 9, 13, -1, -1},
-						{-1, 2, 0, -1, 4, 5, -1},
-						{-1, -1, -1, 0, -1, -1, 1},
-						{1, -1, -1, 1, 0, -1, 3},
-						{-1, -1, -1, -1, 9, 0, 2},
-						{-1, -1, -1, -1, -1, -1, 0}
+				s32 w[size][size] = {
+						{0, 2, 4, 999, 999, 999, 999},
+						{999, 0, 1, 9, 13, 999, 999},
+						{999, 2, 0, 999, 4, 5, 999},
+						{999, 999, 999, 0, 999, 999, 1},
+						{1, 999, 999, 1, 0, 999, 3},
+						{999, 999, 999, 999, 9, 0, 2},
+						{999, 999, 999, 999, 999, 999, 0}
 				};
-				int **weights = new int*[size];
+				s32 **weights = new s32*[size];
 
 				for(int i = 0; i < size; i++){
-						weights[i] = new int[size];
+						weights[i] = new s32[size];
 
 						for(int j = 0; j < size; j++)
 								weights[i][j] = w[i][j];
 				}
 
 				vector<int> path = pathfinder->findPath(weights, size, 0, size - 1);
-				CPPUNIT_ASSERT(path.size() == 6);
+				CPPUNIT_ASSERT(path == vector<int>({0, 1, 2, 4, 3, 6}));
 
 				int sumPathWeights = 0;
 
-				for(int p : path)
-						sumPathWeights += p;
+				for(int i = 1; i < size; i++)
+						sumPathWeights += w[path[i - 1]][path[i]];
 
 				CPPUNIT_ASSERT(sumPathWeights == 9);
 		}