I found this a while back not sure if it was originally written in c or c++... been trying to convert it to work with Arduino. I am just trying to make this bit work and will add some more in the future.