Non working implementation.
Implementation was working, matrixes were wrong.
Merge pull request #1 from jfacorro/first.implementation

First implementation