Finished the test function

Finished Test Function. Not complied on mbed yet. After code is complied
it will pasted back to clean out errors.