Fix bug where new the value of P00 and P01 where used instead of the old
Changed all doubles to float

Also made some variables local variables