diff --git a/.gitignore b/.gitignore
index 428a1df..28de1b5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 .idea
 node_modules
 coverage
+target
\ No newline at end of file
diff --git a/Cargo.lock b/Cargo.lock
new file mode 100644
index 0000000..a6b7534
--- /dev/null
+++ b/Cargo.lock
@@ -0,0 +1,7 @@
+# This file is automatically @generated by Cargo.
+# It is not intended for manual editing.
+version = 4
+
+[[package]]
+name = "oh_pathfinding"
+version = "0.0.0"
diff --git a/Cargo.toml b/Cargo.toml
new file mode 100644
index 0000000..6e5f790
--- /dev/null
+++ b/Cargo.toml
@@ -0,0 +1,14 @@
+[package]
+name = "oh_pathfinding"
+version = "0.0.0"
+edition = "2024"
+
+[lib]
+path = "src/lib.rs"
+crate-type = ["cdylib"]
+
+[dependencies]
+
+[[bin]]
+name = "start"
+path = "src/lib.rs"
\ No newline at end of file
diff --git a/deno.json b/deno.json
index c5da76c..d935744 100644
--- a/deno.json
+++ b/deno.json
@@ -4,5 +4,8 @@
   "exports": "./mod.ts",
   "imports": {
     "std/": "https://deno.land/std@0.224.0/"
+  },
+  "tasks": {
+    "build:lib": "cargo build --release"
   }
 }
diff --git a/deno.lock b/deno.lock
index 6147e1b..186e24d 100644
--- a/deno.lock
+++ b/deno.lock
@@ -1,5 +1,5 @@
 {
-  "version": "3",
+  "version": "4",
   "remote": {
     "https://deno.land/std@0.224.0/assert/_constants.ts": "a271e8ef5a573f1df8e822a6eb9d09df064ad66a4390f21b3e31f820a38e0975",
     "https://deno.land/std@0.224.0/assert/assert.ts": "09d30564c09de846855b7b071e62b5974b001bb72a4b797958fe0660e7849834",
@@ -41,6 +41,7 @@
   "workspace": {
     "packageJson": {
       "dependencies": [
+        "npm:esbuild@0.23.0",
         "npm:prettier@3.3.3"
       ]
     }
diff --git a/preview/bundle.js b/preview/bundle.js
index 415d7df..bec8d5a 100644
--- a/preview/bundle.js
+++ b/preview/bundle.js
@@ -1,8 +1,9 @@
 // src/utils/grid.utils.ts
 var makeSquare = (layout) => {
   const maxLength = Math.max(layout.length, ...layout.map((row) => row.length));
-  const squareLayout = Array.from({ length: maxLength }, () =>
-    Array(maxLength).fill(null),
+  const squareLayout = Array.from(
+    { length: maxLength },
+    () => Array(maxLength).fill(null)
   );
   for (let i = 0; i < layout.length; i++) {
     for (let j = 0; j < layout[i].length; j++) {
@@ -13,8 +14,9 @@ var makeSquare = (layout) => {
 };
 var transpose = (matrix) => {
   const maxCols = Math.max(...matrix.map((row) => row.length));
-  const transposed = Array.from({ length: maxCols }, () =>
-    Array(matrix.length).fill(null),
+  const transposed = Array.from(
+    { length: maxCols },
+    () => Array(matrix.length).fill(null)
   );
   for (let i = 0; i < matrix.length; i++) {
     for (let j = 0; j < matrix[i].length; j++) {
@@ -92,7 +94,7 @@ var findPath = (startPoint, endPoint, grid, config) => {
   const orthogonalCostMultiplier = config.orthogonalCostMultiplier ?? 1;
   const maxJumpCost = config.maxJumpCost ?? 5;
   const maxIterations = config.maxIterations ?? 99999;
-  const jumpDiagonals = config.jumpDiagonals ?? false;
+  const jumpBlockedDiagonals = config.jumpBlockedDiagonals ?? false;
   const index = (point) => {
     return point.y * grid.height + point.x;
   };
@@ -110,7 +112,7 @@ var findPath = (startPoint, endPoint, grid, config) => {
     while (true) {
       const target = {
         x: src.x + dirX * jumpDistance,
-        y: src.y + dirY * jumpDistance,
+        y: src.y + dirY * jumpDistance
       };
       if (!grid.isWalkable(target)) {
         break;
@@ -125,7 +127,7 @@ var findPath = (startPoint, endPoint, grid, config) => {
       if (totalCost < visited[targetIndex]) {
         visited[targetIndex] = totalCost;
         queue.push(
-          new PathNode(target, prevNode, totalCost, heuristic(target)),
+          new PathNode(target, prevNode, totalCost, heuristic(target))
         );
       }
       prevPoint = target;
@@ -137,24 +139,13 @@ var findPath = (startPoint, endPoint, grid, config) => {
   };
   const addDiagonal = (prevNode, src, srcCost, dirX, dirY) => {
     const target = { x: src.x + dirX, y: src.y + dirY };
-    const moveCost =
-      srcCost +
-      (getMoveCostAt(src, target) ?? NOT_REACHED_COST) * diagonalCostMultiplier;
+    const moveCost = srcCost + (getMoveCostAt(src, target) ?? NOT_REACHED_COST) * diagonalCostMultiplier;
     const targetHeight = grid.getHeightAt(target);
     const aux1 = { x: src.x, y: src.y + dirY };
     const aux2 = { x: src.x + dirX, y: src.y };
     const targetIndex = index(target);
-    const canJumpDiagonals =
-      jumpDiagonals ||
-      (grid.isWalkable(aux1) &&
-        grid.isWalkable(aux2) &&
-        targetHeight == grid.getHeightAt(aux1) &&
-        targetHeight == grid.getHeightAt(aux2));
-    if (
-      grid.isWalkable(target) &&
-      canJumpDiagonals &&
-      moveCost < visited[targetIndex]
-    ) {
+    const canJumpDiagonals = jumpBlockedDiagonals || grid.isWalkable(aux1) && grid.isWalkable(aux2) && targetHeight == grid.getHeightAt(aux1) && targetHeight == grid.getHeightAt(aux2);
+    if (grid.isWalkable(target) && canJumpDiagonals && moveCost < visited[targetIndex]) {
       visited[targetIndex] = moveCost;
       queue.push(new PathNode(target, prevNode, moveCost, heuristic(target)));
     }
@@ -253,24 +244,15 @@ var Grid = class _Grid {
     return new _Grid(
       this.width,
       this.height,
-      new Float32Array(this.heightMatrix),
+      new Float32Array(this.heightMatrix)
     );
   }
   inBounds(point) {
-    return (
-      point.x >= 0 &&
-      point.x < this.width &&
-      point.y >= 0 &&
-      point.y < this.height
-    );
+    return point.x >= 0 && point.x < this.width && point.y >= 0 && point.y < this.height;
   }
   isWalkable(point) {
     const heightAt = this.getHeightAt(point);
-    return (
-      this.inBounds(point) &&
-      heightAt !== null &&
-      heightAt !== NON_WALKABLE_HEIGHT
-    );
+    return this.inBounds(point) && heightAt !== null && heightAt !== NON_WALKABLE_HEIGHT;
   }
   walkMatrix(callback) {
     for (let y = 0; y < this.height; y++) {
@@ -291,15 +273,15 @@ var Grid = class _Grid {
     }
     return matrix;
   }
-  findPath(
-    startPoint,
-    endPoint,
-    config = {
-      maxJumpCost: 5,
-      travelCosts: void 0,
-    },
-  ) {
+  findPath(startPoint, endPoint, config = {
+    maxJumpCost: 5,
+    travelCosts: void 0
+  }) {
     return findPath(startPoint, endPoint, this, config);
   }
 };
-export { Grid, makeSquare, transpose };
+export {
+  Grid,
+  makeSquare,
+  transpose
+};
diff --git a/src/find_path_config.rs b/src/find_path_config.rs
new file mode 100644
index 0000000..233208b
--- /dev/null
+++ b/src/find_path_config.rs
@@ -0,0 +1,26 @@
+#[derive(Debug, Clone, Default)]
+pub struct FindPathConfig {
+    pub diagonal_cost_multiplier: f32,
+    pub orthogonal_cost_multiplier: f32,
+    pub max_jump_cost: f32,
+    pub max_iterations: u32,
+    pub jump_blocked_diagonals: bool,
+}
+
+impl FindPathConfig {
+    pub fn new(
+        diagonal_cost_multiplier: Option<f32>,
+        orthogonal_cost_multiplier: Option<f32>,
+        max_jump_cost: Option<f32>,
+        max_iterations: Option<u32>,
+        jump_blocked_diagonals: Option<bool>,
+    ) -> Self {
+        FindPathConfig {
+            diagonal_cost_multiplier: diagonal_cost_multiplier.unwrap_or(1.0),
+            orthogonal_cost_multiplier: orthogonal_cost_multiplier.unwrap_or(1.0),
+            max_jump_cost: max_jump_cost.unwrap_or(5.0),
+            max_iterations: max_iterations.unwrap_or(99999),
+            jump_blocked_diagonals: jump_blocked_diagonals.unwrap_or(false),
+        }
+    }
+}
diff --git a/src/finder.rs b/src/finder.rs
new file mode 100644
index 0000000..1f59740
--- /dev/null
+++ b/src/finder.rs
@@ -0,0 +1,239 @@
+use crate::find_path_config::FindPathConfig;
+use crate::grid::Grid;
+use crate::open_list::{Compare, OpenList};
+use crate::path_node::PathNode;
+use crate::point::Point;
+use std::f32;
+
+#[derive(Debug)]
+pub struct PathNodeComparator;
+
+impl Compare<PathNode> for PathNodeComparator {
+    fn compare(&self, a: &PathNode, b: &PathNode) -> f32 {
+        (a.cost + a.heuristic_value) - (b.cost + b.heuristic_value)
+    }
+}
+
+pub struct Finder {
+    not_reached_cost: f32,
+    visited: Vec<f32>,
+    travel_heuristic: Vec<f32>,
+    queue: OpenList<PathNode>,
+
+    pub start_point: Point,
+    pub end_point: Point,
+    pub grid: Grid,
+    pub config: FindPathConfig,
+}
+
+impl Finder {
+    pub fn new(
+        start_point: Point,
+        end_point: Point,
+        grid: Grid,
+        config: Option<FindPathConfig>,
+    ) -> Self {
+        let not_reached_cost = 999999.0;
+
+        let mut visited = vec![0.0; grid.width * grid.height];
+        visited.fill(not_reached_cost);
+
+        let mut travel_heuristic = vec![0.0; grid.width * grid.height];
+        travel_heuristic.fill(not_reached_cost);
+
+        Finder {
+            not_reached_cost,
+            visited,
+            travel_heuristic,
+            queue: OpenList::new(Box::new(PathNodeComparator)),
+
+            start_point,
+            end_point,
+            grid,
+            config: config.unwrap_or(FindPathConfig::new(None, None, None, None, None)),
+        }
+    }
+
+    fn index(&self, point: &Point) -> usize {
+        (point.y * (self.grid.height as isize) + point.x) as usize
+    }
+
+    fn get_move_cost_at(&self, src: &Point, dst: &Point) -> Option<f32> {
+        if !self.grid.in_bounds(&src) || !self.grid.in_bounds(&dst) {
+            return None;
+        }
+        let src_height = self
+            .grid
+            .get_height_at(&src)
+            .unwrap_or(self.not_reached_cost);
+        let dst_height = self
+            .grid
+            .get_height_at(&dst)
+            .unwrap_or(self.not_reached_cost);
+
+        println!(
+            "{} {}",
+            (src_height - dst_height).abs(),
+            self.config.max_jump_cost
+        );
+        if (src_height - dst_height).abs() > self.config.max_jump_cost {
+            return None;
+        }
+
+        Some(1.0)
+    }
+
+    fn heuristic(&self, point: &Point) -> f32 {
+        self.travel_heuristic[self.index(&point)]
+    }
+
+    fn add_orthogonal_jumps(&mut self, prev_node: &PathNode, dir_x: isize, dir_y: isize) {
+        let mut jump_distance = 1;
+        let mut accumulated_cost = 0f32;
+        let mut prev_point = prev_node.point.clone();
+
+        loop {
+            let target: Point = Point::new(
+                prev_node.point.x + dir_x * jump_distance,
+                prev_node.point.y + dir_y * jump_distance,
+            );
+            if !self.grid.is_walkable(&target) {
+                return;
+            }
+            let move_cost = match self.get_move_cost_at(&prev_point, &target) {
+                Some(cost) => cost,
+                None => return,
+            };
+
+            accumulated_cost += move_cost * self.config.orthogonal_cost_multiplier;
+            let target_index = self.index(&target);
+            let total_cost = prev_node.cost + accumulated_cost;
+
+            if total_cost < self.visited[target_index] {
+                self.visited[target_index] = total_cost;
+                self.queue.push(PathNode::new(
+                    self.start_point.clone(),
+                    None,
+                    0f32,
+                    self.heuristic(&target),
+                ))
+            }
+            prev_point = target;
+            jump_distance = jump_distance + 1;
+
+            if accumulated_cost > self.config.max_jump_cost {
+                return;
+            }
+        }
+    }
+
+    fn add_diagonal(&mut self, prev_node: &PathNode, dir_x: isize, dir_y: isize) {
+        let target: Point = Point {
+            x: prev_node.point.x + dir_x,
+            y: prev_node.point.y + dir_y,
+        };
+        let move_cost = prev_node.cost
+            + (self
+                .get_move_cost_at(&prev_node.point, &target)
+                .unwrap_or(self.not_reached_cost)) as f32
+                * self.config.diagonal_cost_multiplier;
+        let target_height = self.grid.get_height_at(&target);
+        let aux1: Point = Point {
+            x: prev_node.point.x,
+            y: prev_node.point.y + dir_y,
+        };
+        let aux2: Point = Point {
+            x: prev_node.point.x + dir_x,
+            y: prev_node.point.y,
+        };
+        let target_index = self.index(&target);
+
+        let can_jump_diagonals = self.config.jump_blocked_diagonals
+            || (self.grid.is_walkable(&aux1)
+                && self.grid.is_walkable(&aux2)
+                && target_height == self.grid.get_height_at(&aux1)
+                && target_height == self.grid.get_height_at(&aux2));
+
+        if self.grid.is_walkable(&target)
+            && can_jump_diagonals
+            && move_cost < self.visited[target_index]
+        {
+            self.visited[target_index] = move_cost;
+            self.queue.push(PathNode::new(
+                target.clone(),
+                Some(Box::new(prev_node.clone())),
+                move_cost,
+                self.heuristic(&target),
+            ))
+        }
+    }
+
+    fn get_path_from_node(&self, last_node: PathNode) -> Vec<Point> {
+        let mut path: Vec<Point> = Vec::new();
+        let mut node: Option<Box<PathNode>> = Some(Box::new(last_node));
+        loop {
+            match node {
+                None => {
+                    break;
+                }
+                Some(current_node) => {
+                    path.push(current_node.point);
+                    node = current_node.from
+                }
+            }
+        }
+
+        path.reverse();
+        path
+    }
+
+    pub fn find(&mut self) -> Vec<Point> {
+        let empty = Vec::new();
+
+        if !self.grid.is_walkable(&self.start_point) || !self.grid.is_walkable(&self.end_point) {
+            return empty;
+        }
+
+        self.grid.walk_matrix(|x, y, _cost| {
+            self.travel_heuristic[y * self.grid.height + x] = self
+                .grid
+                .distance(&Point::new(x as isize, y as isize), &self.end_point);
+        });
+
+        let start_index = self.index(&self.start_point.clone());
+        self.visited[start_index] = 0f32;
+        self.queue.push(PathNode::new(
+            self.start_point.clone(),
+            None,
+            0f32,
+            self.heuristic(&self.start_point),
+        ));
+
+        let mut iterations = 0;
+
+        loop {
+            if self.queue.is_empty() {
+                return empty;
+            }
+            if iterations > self.config.max_iterations {
+                return empty;
+            }
+            iterations += 1;
+            let node = self.queue.pop().unwrap();
+
+            if node.point.x == self.end_point.x && node.point.y == self.end_point.y {
+                return self.get_path_from_node(node);
+            }
+
+            self.add_orthogonal_jumps(&node, 0, -1);
+            self.add_orthogonal_jumps(&node, 0, 1);
+            self.add_orthogonal_jumps(&node, -1, 0);
+            self.add_orthogonal_jumps(&node, 1, 0);
+
+            self.add_diagonal(&node, 1, 1);
+            self.add_diagonal(&node, -1, 1);
+            self.add_diagonal(&node, 1, -1);
+            self.add_diagonal(&node, -1, -1);
+        }
+    }
+}
diff --git a/src/grid.rs b/src/grid.rs
new file mode 100644
index 0000000..252310b
--- /dev/null
+++ b/src/grid.rs
@@ -0,0 +1,87 @@
+use crate::point::Point;
+use crate::utils::make_square;
+
+#[derive(Debug)]
+pub struct Grid {
+    pub width: usize,
+    pub height: usize,
+    pub height_matrix: Vec<f32>,
+}
+
+impl Grid {
+    pub fn new(width: usize, height: usize, cost_matrix: Vec<f32>) -> Self {
+        Grid {
+            width,
+            height,
+            height_matrix: cost_matrix,
+        }
+    }
+
+    pub fn from(matrix: Vec<Vec<usize>>) -> Result<Self, String> {
+        if matrix.get(0).is_none() || matrix.get(0).unwrap().get(0).is_none() {
+            return Err("Grid matrix cannot be empty!".to_string());
+        }
+
+        let mat = make_square(matrix);
+        let height = mat.len();
+        let width = mat.get(0).unwrap().len();
+
+        if height != width {
+            return Err("Grid matrix must be square!".to_string());
+        }
+
+        let mut cost_matrix = vec![0.0; width * height];
+
+        for (y, row) in mat.iter().enumerate() {
+            for (x, &cost) in row.iter().enumerate() {
+                cost_matrix[y * width + x] = cost as f32
+            }
+        }
+
+        Ok(Grid::new(width, height, cost_matrix))
+    }
+
+    pub fn get_height_at(&self, point: &Point) -> Option<f32> {
+        if !self.in_bounds(&point) {
+            return None;
+        }
+        Some(self.height_matrix[self.index(point)])
+    }
+    pub fn distance(&self, point_a: &Point, point_b: &Point) -> f32 {
+        let dx = (point_a.x - point_b.x) as f32;
+        let dy = (point_a.y - point_b.y) as f32;
+        (dx * dx + dy * dy).sqrt()
+    }
+
+    pub fn index(&self, point: &Point) -> usize {
+        (point.y as usize) * self.height + (point.x as usize)
+    }
+
+    pub fn in_bounds(&self, point: &Point) -> bool {
+        point.x >= 0
+            && point.x < self.width as isize
+            && point.y >= 0
+            && point.y < self.height as isize
+    }
+
+    pub fn is_walkable(&self, point: &Point) -> bool {
+        let height_at = self.get_height_at(&point);
+        self.in_bounds(point)
+	        && height_at.is_some()
+	        //NON WALKABLE HEIGHT
+	        && height_at.unwrap() != 0f32
+    }
+
+    pub fn walk_matrix<F>(&self, mut callback: F)
+    where
+        F: FnMut(usize, usize, Option<f32>),
+    {
+        for y in 0..self.height {
+            for x in 0..self.width {
+                let index = y * self.height + x;
+                let cost = self.height_matrix[index];
+                callback(x, y, Some(cost));
+            }
+        }
+    }
+}
diff --git a/src/grid/finder.ts b/src/grid/finder.ts
index 9811619..0ff1d24 100644
--- a/src/grid/finder.ts
+++ b/src/grid/finder.ts
@@ -1,4 +1,4 @@
-import { Point, FindPathConfig } from "../types/main.ts";
+import { FindPathConfig, Point } from "../types/main.ts";
 import { Grid } from "./grid.ts";
 import { OpenList } from "./openList.ts";
 
@@ -112,16 +112,14 @@ export const findPath = (
     dirY: number,
   ) => {
     const target: Point = { x: src.x + dirX, y: src.y + dirY };
-    const moveCost =
-      srcCost +
+    const moveCost = srcCost +
       (getMoveCostAt(src, target) ?? NOT_REACHED_COST) * diagonalCostMultiplier;
     const targetHeight = grid.getHeightAt(target);
     const aux1: Point = { x: src.x, y: src.y + dirY };
     const aux2: Point = { x: src.x + dirX, y: src.y };
     const targetIndex = index(target);
 
-    const canJumpDiagonals =
-      jumpBlockedDiagonals ||
+    const canJumpDiagonals = jumpBlockedDiagonals ||
       (grid.isWalkable(aux1) &&
         grid.isWalkable(aux2) &&
         targetHeight == grid.getHeightAt(aux1) &&
@@ -143,7 +141,7 @@ export const findPath = (
 
   const visited = new Float32Array(grid.width * grid.height);
   visited.fill(NOT_REACHED_COST);
-  config.travelCosts = visited;
+  // config.travelCosts = visited;
 
   // Distance to the end point, from A*
   const travelHeuristic = new Float32Array(grid.width * grid.height);
@@ -152,7 +150,7 @@ export const findPath = (
   grid.walkMatrix((x, y) => {
     travelHeuristic[y * grid.height + x] = grid.distance({ x, y }, endPoint);
   });
-  config.travelHeuristic = travelHeuristic;
+  // config.travelHeuristic = travelHeuristic;
 
   const heuristic = (a: Point): number => {
     return travelHeuristic[index(a)];
diff --git a/src/grid/openList.ts b/src/grid/openList.ts
index c33a3c7..b8f321a 100644
--- a/src/grid/openList.ts
+++ b/src/grid/openList.ts
@@ -1,4 +1,4 @@
-import { CompFn, ListNode } from "../types/main.ts";
+import type { CompFn, ListNode } from "../types/main.ts";
 
 export class OpenList<T> {
   private start: ListNode<T> | null;
@@ -25,7 +25,8 @@ export class OpenList<T> {
     }
 
     let aux: ListNode<T> = this.start;
-    while (aux.next !== null && this.comparator(value, aux.next.value) > 0) {
+    while (aux.next !== null
+    && this.comparator(value, aux.next.value) > 0) {
       aux = aux.next;
     }
 
diff --git a/src/lib.rs b/src/lib.rs
new file mode 100644
index 0000000..0258d68
--- /dev/null
+++ b/src/lib.rs
@@ -0,0 +1,150 @@
+use crate::finder::Finder;
+use crate::grid::Grid;
+use crate::point::Point;
+use crate::utils::{convert_array, transpose};
+use std::time::Instant;
+
+mod find_path_config;
+mod finder;
+mod grid;
+mod open_list;
+mod path_node;
+mod point;
+mod utils;
+
+// pub fn find_path(
+//     start_point: Point,
+//     end_point: Point,
+//     grid: Grid,
+//     config: FindPathConfig,
+// ) -> Vec<Point> {
+//     Vec::new()
+// }
+
+pub fn main() {
+    let timer = Instant::now();
+    let matrix = [
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 4, 7, 10, 10, 10, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+            0, 0, 0, 0,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 4, 7, 10, 10, 10, 10, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 4, 7, 10, 10, 10, 10, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 4, 7, 10, 10, 10, 10, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 12, 12, 12, 12, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 15, 15, 15, 15, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+        [
+            1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19, 19,
+            19, 19, 19, 19, 19, 19, 19,
+        ],
+    ];
+    let layout = transpose(convert_array(matrix));
+
+    let start = Point::new(3, 3);
+    let end = Point::new(3, 4);
+    let grid = Grid::from(layout);
+
+    let mut finder = Finder::new(start, end, grid.unwrap(), None);
+
+    println!(
+        "[{}]",
+        finder
+            .find()
+            .iter()
+            .map(|p| p.to_string())
+            .collect::<Vec<_>>()
+            .join(", ")
+    );
+    println!("{:?}", timer.elapsed())
+}
diff --git a/src/lib.ts b/src/lib.ts
new file mode 100644
index 0000000..ed92683
--- /dev/null
+++ b/src/lib.ts
@@ -0,0 +1,912 @@
+import { transpose } from "./utils/grid.utils.ts";
+import { Grid } from "./grid/grid.ts";
+
+console.time("test");
+const original = [
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    2,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    1,
+    0,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    4,
+    7,
+    10,
+    10,
+    10,
+    10,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+    0,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    4,
+    7,
+    10,
+    10,
+    10,
+    10,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    4,
+    7,
+    10,
+    10,
+    10,
+    10,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    4,
+    7,
+    10,
+    10,
+    10,
+    10,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    12,
+    12,
+    12,
+    12,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    15,
+    15,
+    15,
+    15,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+  [
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    1,
+    0,
+    0,
+    0,
+    0,
+    0,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+    19,
+  ],
+];
+
+const start = {
+  x: 19,
+  y: 17,
+};
+const end = {
+  x: 19,
+  y: 12,
+};
+
+const layout = transpose(original);
+
+const grid = Grid.from(layout);
+
+const config = {
+  // maxJumpCost: 5,
+  // jumpBlockedDiagonals: Boolean(localStorage.getItem("jumpBlockedDiagonals")),
+};
+const path = grid.findPath(start, end, config);
+
+console.log(path);
+console.timeEnd("test");
diff --git a/src/open_list.rs b/src/open_list.rs
new file mode 100644
index 0000000..3e6845d
--- /dev/null
+++ b/src/open_list.rs
@@ -0,0 +1,128 @@
+use std::cmp::Ordering;
+use std::fmt::Debug;
+
+#[derive(Debug)]
+pub struct ListNode<Value> {
+    pub value: Value,
+    pub next: Option<Box<ListNode<Value>>>,
+}
+
+#[allow(dead_code)]
+impl<Value> ListNode<Value> {
+    fn new(value: Value, next: Option<Box<ListNode<Value>>>) -> Self {
+        ListNode { value, next }
+    }
+
+    fn set_next(&mut self, next: Option<Box<ListNode<Value>>>) {
+        self.next = next;
+    }
+}
+
+pub trait Compare<Value>: Debug {
+    fn compare(&self, a: &Value, b: &Value) -> f32;
+}
+
+#[derive(Debug)]
+pub struct OpenList<Value> {
+    start: Option<Box<ListNode<Value>>>,
+    size: usize,
+    comparator: Box<dyn Compare<Value>>,
+}
+#[allow(dead_code)]
+impl<Value> OpenList<Value> {
+    pub fn new(comparator: Box<dyn Compare<Value>>) -> Self {
+        OpenList {
+            start: None,
+            size: 0,
+            comparator,
+        }
+    }
+    pub fn push(&mut self, value: Value) {
+        if self.start.is_none() {
+            self.start = Some(Box::new(ListNode::new(value, None)));
+            self.size += 1;
+            return;
+        }
+        // Compare with the first element
+        if self.comparator(&value, &self.start.as_ref().unwrap().value) < 0 {
+            let old_start = self.start.take();
+            self.start = Some(Box::new(ListNode::new(value, old_start)));
+            self.size += 1;
+            return;
+        }
+
+        // Traverse to find insertion point
+        let mut current = &mut self.start;
+        while let Some(node) = current {
+            if node.next.is_none() {
+                // Insert at the end
+                node.next = Some(Box::new(ListNode::new(value, None)));
+                self.size += 1;
+                return;
+            }
+            // Compare with next node's value
+            if self.comparator(&value, &node.next.as_ref().unwrap().value) < 0 {
+                // Insert after current node
+                let next = node.next.take();
+                node.next = Some(Box::new(ListNode::new(value, next)));
+                self.size += 1;
+                return;
+            }
+            current = &mut node.next;
+        }
+    }
+
+    pub fn pop(&mut self) -> Result<Value, String> {
+        match self.start.take() {
+            None => Err("popping from an empty list".to_string()),
+            Some(popped) => {
+                self.start = popped.next;
+                self.size -= 1;
+                Ok(popped.value)
+            }
+        }
+    }
+
+    pub fn length(&self) -> usize {
+        self.size
+    }
+
+    pub fn is_empty(&self) -> bool {
+        self.start.is_none()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[derive(Debug)]
+    struct IntComparator;
+
+    impl Compare<i32> for IntComparator {
+        fn compare(&self, a: &i32, b: &i32) -> Ordering {
+            a.cmp(b)
+        }
+    }
+
+    #[test]
+    fn test_open_list() {
+        let comparator = Box::new(IntComparator);
+        let mut list = OpenList::new(comparator);
+
+        assert!(list.is_empty());
+        assert_eq!(list.length(), 0);
+
+        list.push(3);
+        list.push(1);
+        list.push(4);
+
+        assert!(!list.is_empty());
+        assert_eq!(list.length(), 3);
+
+        assert_eq!(list.pop(), Ok(1));
+        assert_eq!(list.pop(), Ok(3));
+        assert_eq!(list.pop(), Ok(4));
+        assert!(list.pop().is_err());
+    }
+}
diff --git a/src/path_node.rs b/src/path_node.rs
new file mode 100644
index 0000000..b5e57e0
--- /dev/null
+++ b/src/path_node.rs
@@ -0,0 +1,20 @@
+use crate::point::Point;
+
+#[derive(Clone)]
+pub struct PathNode {
+    pub point: Point,
+    pub from: Option<Box<PathNode>>,
+    pub cost: f32,
+    pub heuristic_value: f32,
+}
+
+impl PathNode {
+    pub fn new(point: Point, from: Option<Box<PathNode>>, cost: f32, heuristic_value: f32) -> Self {
+        PathNode {
+            point,
+            from,
+            cost,
+            heuristic_value,
+        }
+    }
+}
diff --git a/src/point.rs b/src/point.rs
new file mode 100644
index 0000000..c96def3
--- /dev/null
+++ b/src/point.rs
@@ -0,0 +1,28 @@
+use std::fmt;
+
+#[derive(Debug)]
+pub struct Point {
+    pub x: isize,
+    pub y: isize,
+}
+
+impl Point {
+    pub fn new(x: isize, y: isize) -> Self {
+        Point { x, y }
+    }
+}
+
+impl Clone for Point {
+    fn clone(&self) -> Self {
+        Self {
+            x: self.x,
+            y: self.y,
+        }
+    }
+}
+
+impl fmt::Display for Point {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f, "({}, {})", self.x, self.y)
+    }
+}
diff --git a/src/types/finder.types.ts b/src/types/finder.types.ts
index d404ce9..9513494 100644
--- a/src/types/finder.types.ts
+++ b/src/types/finder.types.ts
@@ -3,7 +3,5 @@ export type FindPathConfig = {
   orthogonalCostMultiplier?: number;
   diagonalCostMultiplier?: number;
   maxIterations?: number;
-  travelCosts?: Float32Array;
-  travelHeuristic?: Float32Array;
   jumpBlockedDiagonals?: boolean;
 };
diff --git a/src/utils.rs b/src/utils.rs
new file mode 100644
index 0000000..ce1ee1a
--- /dev/null
+++ b/src/utils.rs
@@ -0,0 +1,37 @@
+pub fn convert_array(arr: [[i32; 32]; 26]) -> Vec<Vec<usize>> {
+    arr.iter()
+        .map(|inner| inner.iter().map(|&x| x as usize).collect())
+        .collect()
+}
+pub fn make_square(layout: Vec<Vec<usize>>) -> Vec<Vec<usize>> {
+    let max_length = layout
+        .iter()
+        .map(|row| row.len())
+        .max()
+        .unwrap_or(0)
+        .max(layout.len());
+
+    let mut square = vec![vec![0; max_length]; max_length];
+
+    for (i, row) in layout.iter().enumerate() {
+        for (j, &val) in row.iter().enumerate() {
+            square[i][j] = val;
+        }
+    }
+
+    square
+}
+
+pub fn transpose(matrix: Vec<Vec<usize>>) -> Vec<Vec<usize>> {
+    let max_cols = matrix.iter().map(|row| row.len()).max().unwrap_or(0);
+
+    let mut transposed = vec![vec![0; matrix.len()]; max_cols];
+
+    for (i, row) in matrix.iter().enumerate() {
+        for (j, &val) in row.iter().enumerate() {
+            transposed[j][i] = val;
+        }
+    }
+
+    transposed
+}