ControlAlgorithm c code added - there are TODO's in the code with things that need to be changed, expanded, or added