Code cleaned and seems to be working. Still needs to be documented.