Would you be willing to rewrite this in C++ with Rcpp?  It would considerably improve the readability of the code, and hence make it easy to maintain in the long term.