Holyshit I wrote something almost identical to this for an embedded C++ project in around 2010.