Completed initial version. Needs HW testing