Several fixes and attempts to make improvements.

Samo Penic

2016-07-08 c7f52eada2cb8c1695afac6c7beed8fdda669288

1
Original written by Dan Kroll in FORTRAN. Code was then largely adapted and commented by Miha Fosnaric-Fosni. Rewritten into C by Samo Penic-Peni, with help of Miha's extensive explanations.