Added functions for CORDIC
more work on getting functions ready for new interface
fixed up cordic functions