package vector

import (
	"errors"
	"fmt"
)

// explicitly implements Neville's algorithm for three points at very
// particular parameter values: ts[]{0, 1/3, 2/3}, extrapolate to 1.
func Extrapolate(P []Point2d) (*Point2d, error) {
	if len(P) != 3 {
		return nil, errors.New("only works for len(P) == 3")
	}

	// P_{01} = -2 * P_0 + 3 * P_1
	p01 := P[0].ToVector().Scale(-2.0).ToPoint().Add(P[1].ToVector().Scale(3.0))
	// P_{12} = -1 * P_1 + 2 * P_2
	p12 := P[1].ToVector().Scale(-1.0).ToPoint().Add(P[2].ToVector().Scale(2.0))
	// P_{012} = 1/2 * P_{01} + 3/2 * P_{12}
	p012 := p01.ToVector().Scale(-1.0 / 2.0).ToPoint().Add(p12.ToVector().Scale(3.0 / 2.0))
	return &p012, nil
}

// Implements Neville's Algorithm for a slice of points (P) at parameter
// values(ts), to parameter value (t). Returns the point and an error
func Neville(P []Point2d, ts []float64, t float64) (*Point2d, error) {
	if len(P) != len(ts) {
		return nil, errors.New(
			fmt.Sprintf(
				"Incompatable slice lengths len(P): %d != len(ts): %d",
				len(P),
				len(ts),
			),
		)
	}
	Q := []Point2d(P)
	order := len(P) - 1
	for i := 0; i < order; i++ {
		for j := 0; j < order-i; j++ {
			tLeft := ts[i+j+1] - t
			tRight := t - ts[j]
			Q[j] = Q[j].ToVector().Scale(tLeft).ToPoint().Add(P[j+1].ToVector().Scale(tRight))
			Q[j] = Q[j].ToVector().Scale(1.0 / (ts[i+j+1] - ts[j])).ToPoint()

		}
	}
	return &Q[0], nil
}