diff --git a/src/npc.c b/src/npc.c
index ac5cf24..2910198 100644
--- a/src/npc.c
+++ b/src/npc.c
@@ -53,9 +53,38 @@ int npc_append_path(uint16_t x, uint16_t y, NPC *npc)
     npc->ypath[npc->path_length-1] = y;
 }
 
-uint8_t *as_reconstruct_path(uint8_t *came_from, int came_size, uint8_t *current, int curr_size)
+void as_clean(uint8_t *visited, uint8_t *gscore, uint8_t *fscore)
 {
+    free(visited);
+    free(gscore);
+    free(fscore);
+}
 
+int as_reconstruct_path(uint16_t *came_from, int w, int h, uint16_t spos,
+                            uint16_t dest, NPC *npc)
+{
+    if(npc_clear_path(npc)) return 1;
+
+    uint16_t next = came_from[dest];
+
+    int returnv = 0;
+
+    while(true)
+    {
+        if(npc_append_path(next%w, next/h, npc))
+        {
+            returnv = 1;
+            break;
+        }
+        if(next == dest){
+            returnv = 0;
+            break;
+        }
+        next = came_from[next];
+    }
+    free(came_from);
+
+    return returnv;
 }
 
 //Returns non zero error code on failure
@@ -63,60 +92,75 @@ uint8_t *as_reconstruct_path(uint8_t *came_from, int came_size, uint8_t *current
 //(Very much unfinished for now)
 int npc_pathfind(int dest_x, int dest_y, Map *full_map, NPC *npc)
 {
+    int i, j;
+
     uint32_t w = full_map->w;
     uint32_t h = full_map->h;
-    uint32_t x = npc->x;
-    uint32_t y = npc->y;
+    uint32_t x = floor(npc->curx);
+    uint32_t y = floor(npc->cury);
     uint32_t spos = y*w+x;
     uint8_t *map = full_map->walkable;
     if(map[spos]) return 2;
+    if(map[dest_y*w+dest_x]) return 2;
 
     npc_clear_path(npc);
 
-    uint8_t *came_from = malloc(w*h);
+    uint8_t *visited = malloc(w*h);
+    for(i=0; i<w*h; i++) visited[i] = 1;
+
+    int16_t *came_from = malloc(w*h*2);
+    for(i=0; i<w*h; i++) came_from[i] = -1;
 
     uint8_t *gscore = malloc(w*h);
+    for(i=0; i<w*h; i++) fscore[i] = 255;
     gscore[spos] = 0;
 
     uint8_t *fscore = malloc(w*h);
+    for(i=0; i<w*h; i++) fscore[i] = 255;
     fscore[spos] = 0;
 
-    int tx = x;
-    int ty = y;
     int tpos = spos;
-    int bx;
-    int by;
-    int i, j;
+    uint8_t bscore;
+    int bx = x;
+    int by = y;
 
     for(int iter=0; iter < 1024; iter++)
     {
-        //Check surronding tiles for cost
-        bx = x-1;
-        by = y-1;
-        for(i = tx-1; i < tx+1; i++)
+        bscore = 255;
+        //Cheapest known tile
+        for(i = 0; i < w*h; i++)
         {
-            //Out of bounds (admissible)
-            if(i < 0) continue;
-            //Out of bounds (unforgiveable)
-            if(i > w) break;
-            for(j = ty-1; j < ty+1; j++)
-            {
-                //Out of bounds (admissible)
-                if(j < 0) continue;
-                //Out of bounds (joever for it)
-                if(j > h) break;
-                //Has collision
-                if(map[j*w+i]) continue;
-                if(i == dest_x && j == dest_y){
-                    return npc_append_path(i, j, npc);
-                }
-                //Calculate score (direct distance)
-                int deltX = tx-dest_x;
-                int deltY = ty-dest_y;
-                int tscore = sqrtf(deltX*deltX+deltY*deltY);
-            }
+            if(visited[i]) continue;
+            if(fscore[i] > bscore) continue;
+            bx = i%w;
+            by = i/w;
+            bscore = fscore[i];
+        }
+        if(bx == dest_x && by == dest_y)
+        {
+            as_clean(visited, gscore, fscore);
+            return as_reconstruct_path(came_from, w, h, spos, dest_y*w+dest_x, npc);
         }
 
+        visited[by*w+bx] = 1;
+
+        int att_score;
+
+        for(i = bx-1; i < bx+1; i++)
+        {
+                for(j = by-1; j < by+1; j++)
+                {
+                    if(map[j*w+i]) continue;
+                    att_score = gscore[by*w+bx] + lenght(bx-i, by-j);
+                    if(att_score < gscore[j*w+i])
+                    {
+                        came_from[j*w+i] = by*w+bx;
+                        gscore[j*w+i] = att_score;
+                        fscore[j*w+i] = att_score + lenght(i-dest_x, j-dest_y);
+                        if(visited[j*w+i]) visited[j*w+i] = 0;
+                    }
+                }
+        }
     }
     return 1;
 }