Ops! Sorry :D !
Anyway, I think I can still help. I saw the code have a lot of "ifs", and it is probably slowing the result. I think the natural speed up is done replacing the conditionals for texture acesses and masking. Think this p.x, p.y, A, B, etc as texture coordinates and precompute...