Added Unit Tests

Coverage up to 76%, next step get bluetooth test working