Fortran code could by better, but I have no time :( ...