Fixed bug and improved cos slightly
Added some missing functionality