Finished draft of modularized code.
Fixed bug in trajectory generation code.