Maths for calculating vectors needs checking. Heading needs chekcing too.