diff --git a/astar.go b/astar.go
index 05dd197..eea8eaf 100644
--- a/astar.go
+++ b/astar.go
@@ -71,9 +71,11 @@ func (a *AStar) Init(width int, height int) {
 	}
 }
 
-func (a *AStar) ResetGrid() {
+func (a *AStar) ResetGrid(withTypes bool) {
 	for i := range a.gScores {
-		a.gridTypes[i] = 0
+		if withTypes {
+			a.gridTypes[i] = 0
+		}
 		a.gScores[i] = math.MaxFloat32
 		a.parents[i] = 0
 		a.closedSet[i] = false
@@ -91,6 +93,23 @@ func (a *AStar) RebuildGrid(width int, height int) {
 	a.height = height
 }
 
+func (a *AStar) SetHeuristic(heuristic int32) {
+	switch heuristic {
+	case 0:
+		a.heuristic = func(x int, y int, endX int, endY int) float32 {
+			return float32(math.Abs(float64(x-endX)) + math.Abs(float64(y-endY))) // Manhattan distance
+		}
+	case 1:
+		a.heuristic = func(x int, y int, endX int, endY int) float32 {
+			return float32(math.Sqrt(float64(x-endX)*float64(x-endX) + float64(y-endY)*float64(y-endY))) // Euclidean distance
+		}
+	case 2:
+		a.heuristic = func(x int, y int, endX int, endY int) float32 {
+			return float32(math.Max(float64(x-endX), float64(y-endY))) // Chebyshev distance
+		}
+	}
+}
+
 func (a *AStar) SetGridType(x int, y int, gridType byte) {
 	/*
 		0 = empty
@@ -105,6 +124,10 @@ func (a *AStar) GetGridType(x int, y int) byte {
 	return a.gridTypes[y*a.width+x]
 }
 
+func (a *AStar) GetGridTypes() []byte {
+	return a.gridTypes
+}
+
 func (a *AStar) SetGScores(x int, y int, gScore float32) {
 	a.gScores[y*a.width+x] = gScore
 }
@@ -188,7 +211,6 @@ func (a *AStar) CalculatePath(startX int, startY int, endX int, endY int) [][]in
 				x, y := a.ParentIndexToXY(currentIndex%a.width, currentIndex/a.width, a.parents[currentIndex])
 				path = append(path, []int{x, y})
 			}
-			// path = append(path, []int{startX, startY}) preserve start position
 			return path
 		}
 
@@ -199,6 +221,7 @@ func (a *AStar) CalculatePath(startX int, startY int, endX int, endY int) [][]in
 				continue
 			}
 			if a.gridTypes[neighborIndex] == 1 {
+				a.gScores[neighborIndex] = math.MaxFloat32
 				continue
 			}
 			terrainCost := a.GetTerrainCost(neighborIndex%a.width, neighborIndex/a.width)
diff --git a/main.go b/main.go
index 5106546..23e63d1 100644
--- a/main.go
+++ b/main.go
@@ -85,6 +85,11 @@ func main() {
 	toolDropdownOpen := false
 	textureNeedsUpdate := true
 
+	heuristicOptions := []string{"Manhattan", "Euclidean", "Chebyshev"}
+	heuristicOptionsText := strings.Join(heuristicOptions, ";")
+	activeHeuristic := int32(0)
+	heuristicDropdownOpen := false
+
 	cellSize := float32(25)
 
 	lastMousePos := rl.NewVector2(-1, -1)
@@ -251,6 +256,8 @@ func main() {
 				mapImage = rl.GenImageColor(width, height, rl.NewColor(240, 240, 240, 255))
 				mapTexture = rl.LoadTextureFromImage(mapImage)
 				astar.RebuildGrid(width, height)
+				startPos = rl.NewVector2(-1, -1)
+				endPos = rl.NewVector2(-1, -1)
 			}
 		}
 
@@ -260,13 +267,38 @@ func main() {
 			toolDropdownOpen = !toolDropdownOpen
 		}
 
+		// Heuristic Selector
+		if !toolDropdownOpen {
+			rg.Label(rl.NewRectangle(sidebarX+(10*scale), (135*scale), (180*scale), (30*scale)), "Heuristic:")
+			if rg.DropdownBox(rl.NewRectangle(sidebarX+(10*scale), (160*scale), (180*scale), (30*scale)), heuristicOptionsText, &activeHeuristic, heuristicDropdownOpen) {
+				heuristicDropdownOpen = !heuristicDropdownOpen
+			}
+		}
+
 		// Calculate Path Button
 		if rg.Button(rl.NewRectangle(sidebarX+(10*scale), (screenHeight-(40*scale)), (180*scale), (30*scale)), "Calculate Path") {
-			astar.ResetGrid()
+			astar.ResetGrid(false) // keep grid types, otherwise it will delete the board before simulating
+			astar.SetHeuristic(activeHeuristic)
+			gridTypes := astar.GetGridTypes()
+			for i, gridType := range gridTypes {
+				// reset the map image
+				switch gridType {
+				case 0:
+					rl.ImageDrawPixel(mapImage, int32(i%width), int32(i/width), rl.NewColor(240, 240, 240, 255))
+				case 1:
+					rl.ImageDrawPixel(mapImage, int32(i%width), int32(i/width), rl.NewColor(0, 0, 0, 255))
+				case 2:
+					rl.ImageDrawPixel(mapImage, int32(i%width), int32(i/width), rl.NewColor(0, 255, 0, 255))
+				case 3:
+					rl.ImageDrawPixel(mapImage, int32(i%width), int32(i/width), rl.NewColor(255, 0, 0, 255))
+				}
+			}
 			path := astar.CalculatePath(int(startPos.X), int(startPos.Y), int(endPos.X), int(endPos.Y))
 			fmt.Println(path)
 			for _, p := range path {
-				rl.ImageDrawPixel(mapImage, int32(p[0]), int32(p[1]), rl.NewColor(255, 255, 0, 255))
+				if p[0] != int(startPos.X) || p[1] != int(startPos.Y) { // we want to keep the start position green
+					rl.ImageDrawPixel(mapImage, int32(p[0]), int32(p[1]), rl.NewColor(255, 255, 0, 255))
+				}
 			}
 			textureNeedsUpdate = true
 		}