Complete rework of the code to match requirements. Included changes: New code, Own functions, More comments