つづき。
思わずxor使ってしまったけど、

if (p1=1) & (p1 ^ p1_p) { 〜
の部分が成立するのは、p1=1、p1_p=0の場合だけだから

if (p1=1) & (p1_p=0) { 〜
と書いたほうが分かりやすかったな。ｽﾏﾝ