Added straighten.m

* Is VERY slow currently
* But works