ENH: First working version

Still some probelms left.