Improved algorithm, cleaned code and added more Documentation