16x2 LCD support, self test added, optimizations