Cleaned up the code, fixed som numeric inconsistencies and Wall warnings are gone