Algorithm works now. Need more points.