Few days of work in this, finally altering the correct points, added tests