diff --git a/pathfinder.cpp b/pathfinder.cpp
index b693387..7054230 100644
--- a/pathfinder.cpp
+++ b/pathfinder.cpp
@@ -25,44 +25,55 @@ namespace battleship{
 			return heuristics;
 		}
 
-		vector<int> Pathfinder::findPath(vector<Map::Cell> &cells, vector<float> &heuristics, int source, int dest, Vehicle *vehicle){
+		vector<int> Pathfinder::findPath(vector<Map::Cell> &cells, vector<float> &heuristics, int source, int dest, int vehicleType){
 			if(heuristics.empty() || (heuristics.size() == 1 && heuristics[0] == 0.0))
 				heuristics = calcHeuristics(cells, dest);
 
+			bool useHeur = !heuristics.empty();
+
 			const int size = cells.size();
 			u32 *distances = new u32[size];
 			vector<int> *paths = new vector<int>[size];
-			paths[source].push_back(source);
-
 			vector<pair<int, bool>> cellsByCheck;
-
-			vector<bool> cellChecked;
+			vector<bool> posMinCellChecked;
+			vector<int> possibleMinCells = vector<int>{source};
 
 			for(int i = 0; i < size; i++){
 				cellsByCheck.push_back(pair(i, false));
-				distances[i] = (i == source ? 0 : impassibleNodeVal);
-				cellChecked.push_back(false);
+				distances[i] = impassibleNodeVal;
+				posMinCellChecked.push_back(false);
 			}
 
-			bool useHeur = !heuristics.empty();
-			vector<int> possibleMinCells = vector<int>{source};
-			cellChecked[source] = true;
+			paths[source].push_back(source);
+			distances[source] = 0;
+			posMinCellChecked[source] = true;
+
+			int lastVertStrich = -1;
 
 			while(!cellsByCheck[dest].second){
-				int vertStrich = possibleMinCells[0], vsId = 0;
+				int posMinCellId = 0, vertStrich = possibleMinCells[posMinCellId];
 
 				for(int i = 0; i < possibleMinCells.size(); i++){
 					float sum1 = distances[possibleMinCells[i]] + (useHeur ? heuristics[possibleMinCells[i]] : 0); 
-					float sum2 = distances[possibleMinCells[vsId]] + (useHeur ? heuristics[possibleMinCells[vsId]] : 0); 
+					float sum2 = distances[possibleMinCells[posMinCellId]] + (useHeur ? heuristics[possibleMinCells[posMinCellId]] : 0); 
 
 					if(sum1 < sum2 || (useHeur && sum1 == sum2 && heuristics[i] < heuristics[vertStrich])){
+						posMinCellId = i;
 						vertStrich = possibleMinCells[i];
-						vsId = i;
 					}
 				}
 
-				cellChecked[possibleMinCells[vsId]] = false;
-				possibleMinCells.erase(possibleMinCells.begin() + vsId);
+				if(distances[vertStrich] == impassibleNodeVal){
+					vector<int> path = paths[lastVertStrich];
+
+					delete[] paths;
+					delete[] distances;
+
+					return path;
+				}
+
+				posMinCellChecked[possibleMinCells[posMinCellId]] = false;
+				possibleMinCells.erase(possibleMinCells.begin() + posMinCellId);
 
 				cellsByCheck[vertStrich].second = true;
 
@@ -73,14 +84,20 @@ namespace battleship{
 
 					if(cellsByCheck[edgeNode].second) continue;
 
-					if(!cellChecked[edgeNode]){
-						cellChecked[edgeNode] = true;
+					if(!posMinCellChecked[edgeNode]){
+						posMinCellChecked[edgeNode] = true;
 						possibleMinCells.push_back(edgeNode);
 					}
 
-					bool canMoveToStrichCell = true;
+					UnitType ut = (UnitType)vehicleType;
+					Map::Cell::Type ct = cells[edgeNode].type;
+					bool ship = (ut == UnitType::UNDERWATER || ut == UnitType::SEA_LEVEL);
+					int weightMult = 1;
 
-					if(vehicle){
+					if((ut == UnitType::LAND && ct == Map::Cell::WATER) || (ship && ct == Map::Cell::LAND))
+						weightMult = 10;
+					/*
+					if(vehicleType != -1){
 						UnitType unitType = vehicle->getType();
 						bool ship = (unitType == UnitType::UNDERWATER || unitType == UnitType::SEA_LEVEL);
 						Unit *blockingUnit = cells[vertStrich].blockedBy;
@@ -106,13 +123,16 @@ namespace battleship{
 							continue;
 						}
 					}
+					*/
 
-					if(distances[vertStrich] + cells[vertStrich].edges[i].weight < distances[edgeNode]){
-						distances[edgeNode] = distances[vertStrich] + cells[vertStrich].edges[i].weight;
+					if(distances[vertStrich] + weightMult * cells[vertStrich].edges[i].weight < distances[edgeNode]){
+						distances[edgeNode] = distances[vertStrich] + weightMult * cells[vertStrich].edges[i].weight;
 						paths[edgeNode] = paths[vertStrich];
 						paths[edgeNode].push_back(edgeNode);
 					}
 				}
+
+				lastVertStrich = vertStrich;
 			}
 
 			vector<int> path = paths[dest];
diff --git a/pathfinder.h b/pathfinder.h
index 7a63c33..66a9000 100644
--- a/pathfinder.h
+++ b/pathfinder.h
@@ -14,7 +14,7 @@ namespace battleship{
 		public:
 			static Pathfinder* getSingleton();
 			std::vector<float> calcHeuristics(std::vector<Map::Cell>&, int);
-			std::vector<int> findPath(std::vector<Map::Cell>&, std::vector<float>&, int, int, Vehicle* = nullptr);
+			std::vector<int> findPath(std::vector<Map::Cell>&, std::vector<float>&, int, int, int = -1);
 			inline vb01::u32 getImpassibleNodeVal(){return impassibleNodeVal;}
 			inline void setImpassibleNodeVal(vb01::u32 val){this->impassibleNodeVal = val;}
 		private:
diff --git a/pathfinderTest.cpp b/pathfinderTest.cpp
index 5bb56ed..362fc75 100644
--- a/pathfinderTest.cpp
+++ b/pathfinderTest.cpp
@@ -1,5 +1,6 @@
 #include "pathfinderTest.h"
 #include "pathfinder.h"
+#include "unit.h"
 
 #include <vector.h>
 #include <util.h>
@@ -61,9 +62,6 @@ namespace battleship{
 				Map::Cell(Vector3::VEC_ZERO, Map::Cell::Type::LAND, vector<Map::Edge>{Map::Edge(0, 6, 6)})
 			};
 
-			const u16 INF = u16(0 - 1);
-			pathfinder->setImpassibleNodeVal(INF);
-
 			int src = 0, dest = cells.size() - 1;
 			vector<float> heur;
 			vector<int> path = pathfinder->findPath(cells, heur, src, dest);
@@ -93,8 +91,27 @@ namespace battleship{
 			CPPUNIT_ASSERT(sumTime <= threshold);
 		}
 
+		void PathfinderTest::testFindShorePath(){
+			int numSideCells = 25;
+			cells = generateCellGraph(numSideCells);
+
+			for(int i = 0; i < numSideCells; i++)
+				cells[numSideCells * i + int(.5 * numSideCells)].type = Map::Cell::WATER;
+
+			vector<float> heur = vector<float>{};
+			vector<int> p1 = pathfinder->findPath(cells, heur, 0, 4, (int)UnitType::LAND);
+
+			int numEdges = cells[p1[p1.size() - 1]].edges.size();
+			//for(int i = 0; i < numEdges; i++){}
+
+			vector<int> p2 = pathfinder->findPath(cells, heur, 2, 4, (int)UnitType::SEA_LEVEL);
+			CPPUNIT_ASSERT(cells[p2[p2.size() - 1]].type == Map::Cell::WATER);
+		}
+
 		void PathfinderTest::setUp(){
 			pathfinder = Pathfinder::getSingleton();
+			const u16 INF = u16(0 - 1);
+			pathfinder->setImpassibleNodeVal(INF);
 		}
 
 		void PathfinderTest::tearDown(){}
diff --git a/pathfinderTest.h b/pathfinderTest.h
index 0e67730..b1c4d27 100644
--- a/pathfinderTest.h
+++ b/pathfinderTest.h
@@ -13,12 +13,14 @@ namespace battleship{
 		CPPUNIT_TEST_SUITE(PathfinderTest);
 		CPPUNIT_TEST(testFindPath);
 		CPPUNIT_TEST(testFindBigPath);
+		CPPUNIT_TEST(testFindShorePath);
 		CPPUNIT_TEST_SUITE_END();
 
 		public:
 			PathfinderTest(){}
 			void testFindPath();
 			void testFindBigPath();
+			void testFindShorePath();
 			void setUp();
 			void tearDown();
 		private:
diff --git a/vehicle.cpp b/vehicle.cpp
index c4085b6..813b339 100644
--- a/vehicle.cpp
+++ b/vehicle.cpp
@@ -371,7 +371,7 @@ namespace battleship{
 		bool ship = (type == UnitType::UNDERWATER || type == UnitType::SEA_LEVEL);
 		vector<Map::Cell> &cells = Map::getSingleton()->getCells();
 
-		for(int i = 0; i < path.size(); i++){
+		for(int i = 1; i < path.size(); i++){
 			if((ship && cells[path[i]].type != Map::Cell::WATER) || (order.type != Order::TYPE::GARRISON && type == UnitType::LAND && cells[path[i]].type != Map::Cell::LAND)){
 				path = vector<int>(path.begin(), path.begin() + i);
 				order.targets[0].unit = nullptr;
@@ -443,13 +443,11 @@ namespace battleship{
 
 		vector<float> heurs;
 		Pathfinder *pf = Pathfinder::getSingleton();
-		vector<int> path = pf->findPath(cells, heurs, source, dest, this);
+		vector<int> path = pf->findPath(cells, heurs, source, dest, (int)type);
 
-		if(path.empty()) return;
+		if(path.empty() || !truncatePath(order, path, destPos, appendDestPos)) return;
 
 		path.erase(path.begin());
-
-		if(!truncatePath(order, path, destPos, appendDestPos)) return;
 	}
 
 	void Vehicle::removePathpoint(int i){