Corrected bath in heatbath algorithm and reduced number of matrix variables in functions
Merged from master