Added preliminary code, seems to have a few mathsy bugs