Modularize, separate to classes and add tests