debayer.cpp implemented, doing testbench now.