Optimization help regarding a custom pathfinding script thing

I told you already that your code is not type safe already.
You have to fix code by giving strict types on everything and doing type-casting is nessesary.