Added the actual code

Added the actual header with the implementation, and the test program.