Finished initial code, needs optimising to avoid fp overflow on r & q