Fixed cosf=nan, working very well now