Basic algorithm working. Test cases created. Code pretty ugly