After month of work and a lot of fail I finally got a fully functional custom brick that work as well as a Lego.