Lookup B4R inline c - you can put the whole thing as an inline c (change setup() to setup2() or init() and call that from B4R with the RunNative function). It looks like the code is also interrupt driven so whatever additional features you add to it you'll need to factor that in.