Corrected encode function (unoptimized PoC)

The encode function had one incorrect statement that messed it up. Now
fixed for PoC.