Updated code

Better algorithms, better comments