reorganized subpixel code. Need to check if the calculation is REALLY correct