Moved the delay and weighting out of the x0 for loop
finished the WFS 2.5d driving function