diff --git a/C++ProjectTemplate b/C++ProjectTemplate
index 12d4820..878ea37 100755
Binary files a/C++ProjectTemplate and b/C++ProjectTemplate differ
diff --git a/main.cpp b/main.cpp
index 083d997..ab90e67 100644
--- a/main.cpp
+++ b/main.cpp
@@ -7,6 +7,15 @@ std::map<std::string,bool>blocks;
 std::map<std::string,bool>trappedBlocks;
 int minX=0,maxX=0,minY=0,maxY=0,minZ=0,maxZ=0;
 
+struct node{
+    int x,y,z;
+    bool visited=false;
+    std::vector<node*>connections;
+    int myDist=INFINITY;
+    int storedDist=INFINITY;
+    node*parent=nullptr;
+};
+
 bool escape(int x,int y,int z,std::map<std::string,bool>visited){
     visited[std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z)]=true;
     if (x<=minX||x>=maxX||y<=minY||y>=maxY||z<=minZ||z>=maxZ){
@@ -15,12 +24,26 @@ bool escape(int x,int y,int z,std::map<std::string,bool>visited){
         if (blocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z))!=blocks.end()){
             return false;
         } 
-        if (visited.find(std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z))==visited.end()&&escape(x+1,y,z,visited)||
-            visited.find(std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z))==visited.end()&&escape(x-1,y,z,visited)||
-            visited.find(std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z))==visited.end()&&escape(x,y+1,z,visited)||
-            visited.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==visited.end()&&escape(x,y-1,z,visited)||
-            visited.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1))==visited.end()&&escape(x,y,z+1,visited)||
-            visited.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1))==visited.end()&&escape(x,y,z-1,visited)){
+        bool visitedSections[]={visited.find(std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z))==visited.end(),
+            visited.find(std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z))==visited.end(),
+            visited.find(std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z))==visited.end(),
+            visited.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==visited.end(),
+            visited.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1))==visited.end(),
+            visited.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1))==visited.end(),};
+
+        visited[std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z)]=true;
+        visited[std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z)]=true;
+        visited[std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z)]=true;
+        visited[std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z)]=true;
+        visited[std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1)]=true;
+        visited[std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1)]=true;
+        
+        if (!visitedSections[0]&&escape(x+1,y,z,visited)||
+            !visitedSections[1]&&escape(x-1,y,z,visited)||
+            !visitedSections[2]&&escape(x,y+1,z,visited)||
+            !visitedSections[3]&&escape(x,y-1,z,visited)||
+            !visitedSections[4]&&escape(x,y,z+1,visited)||
+            !visitedSections[5]&&escape(x,y,z-1,visited)){
             return true;
         } else {
             return false;
@@ -30,8 +53,9 @@ bool escape(int x,int y,int z,std::map<std::string,bool>visited){
 
 int main()
 {
-	std::ifstream file("input");
+	std::ifstream file("testinput");
     int surfaceArea=0;
+
     while (file.good()){
         std::string line;
         std::getline(file,line);
@@ -44,12 +68,12 @@ int main()
             marker=line.find(',',marker)+1;
             int z=std::atoi(line.substr(marker,line.find(',',marker)-marker).c_str());
             std::cout<<x<<","<<y<<","<<z<<std::endl;
-            minX=std::min(x,minX);
-            maxX=std::max(x,maxX);
-            minY=std::min(y,minY);
-            maxY=std::max(y,maxY);
-            minZ=std::min(z,minZ);
-            maxZ=std::max(z,maxZ);
+            minX=std::min(x-1,minX);
+            maxX=std::max(x+1,maxX);
+            minY=std::min(y-1,minY);
+            maxY=std::max(y+1,maxY);
+            minZ=std::min(z-1,minZ);
+            maxZ=std::max(z+1,maxZ);
             if (blocks.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==blocks.end()){
                 surfaceArea++;
             } else {
@@ -84,42 +108,129 @@ int main()
             std::cout<<surfaceArea<<std::endl;
         }
     }
+
+    std::vector<std::vector<std::vector<node>>>grid;
+    for(int z=minZ;z<=maxZ;z++){
+        std::vector<std::vector<node>>minigrid;
+        for(int y=minY;y<=maxY;y++){
+            std::vector<node>miniminigrid;
+            for(int x=minX;x<=maxX;x++){
+                node myNode{x,y,z};
+                miniminigrid.push_back(myNode);
+            }
+            minigrid.push_back(miniminigrid);
+        }
+        grid.push_back(minigrid);
+    }
+    for (int x=-minX;x<=maxX;x++){
+        for (int y=-minY;y<=maxY;y++){
+            for (int z=-minZ;z<=maxZ;z++){
+                node&myNode=grid[z-minZ][y-minY][x-minX];
+                if(x+1<=maxX&&blocks.find(std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z))==blocks.end()){
+                    myNode.connections.push_back(&grid[z-minZ][y-minY][(x+1)-minX]);
+                }
+                if(x-1>=minX&&blocks.find(std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z))==blocks.end()){
+                    myNode.connections.push_back(&grid[z-minZ][y-minY][(x-1)-minX]);
+                }
+                if(y+1<=maxY&&blocks.find(std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z))==blocks.end()){
+                    myNode.connections.push_back(&grid[z-minZ][y+1-minY][(x)-minX]);
+                }
+                if(y-1>=minY&&blocks.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==blocks.end()){
+                    myNode.connections.push_back(&grid[z-minZ][y-1-minY][(x)-minX]);
+                }
+                if(z+1<=maxZ&&blocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1))==blocks.end()){
+                    myNode.connections.push_back(&grid[z+1-minZ][y-minY][(x)-minX]);
+                }
+                if(z-1>=minZ&&blocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1))==blocks.end()){
+                    myNode.connections.push_back(&grid[z-1-minZ][y-minY][(x)-minX]);
+                }
+            }
+        }
+    }
     int trappedSurfaceArea=0;
     for (int x=-minX;x<=maxX;x++){
         for (int y=-minY;y<=maxY;y++){
             for (int z=-minZ;z<=maxZ;z++){
-                if (!escape(x,y,z,{})){
-                    trappedBlocks[std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z)]=true;
-                    std::cout<<"Block "<<std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z)<<" is trapped"<<std::endl;
-                    if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
-                    }
-                    if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
+                if (blocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z))==blocks.end()){
+                    std::vector<node*>searchNodes;
+                    for (int zz=0;zz<grid.size();zz++){
+                        for (int yy=0;yy<grid[zz].size();yy++){
+                            for (int xx=0;xx<grid[zz][yy].size();xx++){
+                                node&n=grid[zz][yy][xx];
+                                n.visited=false;
+                                n.myDist=INFINITY;
+                                n.parent=nullptr;
+                            }
+                        }
                     }
-                    if (trappedBlocks.find(std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
+                    node*startNode=&grid[z-minZ][y-minY][x-minX];
+                    node*endNode=&grid[0][0][0];
+                    startNode->myDist=0.0f;
+                    searchNodes.push_back(startNode);
+                    while (searchNodes.size()>0){
+                        node*currentNode=searchNodes.front();
+                        if (currentNode->visited){
+                            searchNodes.erase(searchNodes.begin());
+                            continue;
+                        }
+                        currentNode->visited=true;
+                        for (int i=0;i<currentNode->connections.size();i++){
+                            node*neighbor=currentNode->connections[i];
+                            if (!neighbor->visited){
+                                searchNodes.push_back(neighbor);
+                            }
+                            float lowerGoal=currentNode->myDist+pow(neighbor->x-currentNode->x,2)+pow(neighbor->y-currentNode->y,2)+pow(neighbor->z-currentNode->z,2);
+                            if (lowerGoal<neighbor->myDist){
+                                neighbor->parent=currentNode;
+                                neighbor->myDist=lowerGoal;
+                            }
+                        }
                     }
-                    if (trappedBlocks.find(std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
-                    }
-                    if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
+
+                    node*searchNode=endNode;
+                    int jumps=0;
+                    while (searchNode!=startNode){
+                        if (searchNode->parent!=nullptr){
+                            std::cout<<"Node goes from "<<searchNode->x<<","<<searchNode->y<<","<<searchNode->z<<" to "<<searchNode->parent->x<<","<<searchNode->parent->y<<","<<searchNode->parent->z<<std::endl;
+                            jumps++;
+                            searchNode=searchNode->parent;
+                        } else {
+                            break;
+                        }
                     }
-                    if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1))==trappedBlocks.end()){
-                        trappedSurfaceArea++;
-                    } else {
-                        trappedSurfaceArea--;
+                    if (searchNode!=startNode){ //Couldn't find it.
+                        trappedBlocks[std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z)]=true;
+                        std::cout<<"Block "<<std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z)<<" is trapped"<<std::endl;
+                        if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y-1)+"_"+std::to_string(z))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
+                        if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y+1)+"_"+std::to_string(z))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
+                        if (trappedBlocks.find(std::to_string(x-1)+"_"+std::to_string(y)+"_"+std::to_string(z))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
+                        if (trappedBlocks.find(std::to_string(x+1)+"_"+std::to_string(y)+"_"+std::to_string(z))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
+                        if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z-1))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
+                        if (trappedBlocks.find(std::to_string(x)+"_"+std::to_string(y)+"_"+std::to_string(z+1))==trappedBlocks.end()){
+                            trappedSurfaceArea++;
+                        } else {
+                            trappedSurfaceArea--;
+                        }
                     }
                 }
             }