Cleaned up code, mostly just pulled from the paper now