Starting to optimize gcode but still got many bugs in the coordinate cleaning