Cleaned up code, added writing data to file (csv)