Began the C code for the MSP. Needs lots of work...