Implement setLedToColor function.

Add todos for Shiftlights project.
Do some optimization for some functions.