private static double cost(int fh, int sh, double sx, int sy, int ex, int ey){
   return Math.sqrt(Math.pow(sx - ex, 2) + Math.pow(sy - ey, 2)) + Math.abs(fh - sh);
}