Snake Robot Serpentine Motion

Implementation of a IEEE paper based robotic snake model and controller for second year uni project

I have attached my work implementing a robotic snake model described in an IEEE paper (also attached). I am not the most efficient coder, but feel free to have a look and tinker with it slash suggest improvements.
Sorry, its not the most well commented code as well. But if you want understanding of where things come from, please refer to the paper. But I have tested it and it works. I also made animations that run by taking the data from the simulink models.

I should have done this from the start; but I would like to change it to make a n link system. At the moment I have hard coded for a three link snake. It is a spaghetti code but could do with some suggestions and matlab tips.