Added an optimized version (cleaner code)

More cleaner code of my statechart