Reduce stack usage and code dupplication, solve gfortran warnings