Added find eigen3
Fixed CMakeLists
Hard debugging, wip