Working on implementation and debugging
Added testing makefile and test code