Functional code added

The code is functional but nothing more. Needs documentation and testing.