Improve interpolation test, add basic algorithm