implemented c-like code
rewrote in a more matlab-ish way