Initial code commit. Nothing tested yet.

No testing done. Still waiting on some hardware components for debug.