Tied up code

Using smart pointers and algorithms for cleaner code.