using Interpolations
using Plots
plotly()

# Example
A_x = 1.:2.:40.
A = [log(x) for x in A_x]
itp = interpolate(A, BSpline(Cubic(Line(OnGrid()))))

sitp = scale(itp, A_x)

sitp(3.0)
sitp(3.5)

x = convert(Vector{Float64}, range(1.0, 39.0, step=0.25))
ỹ = sitp.(x)
y = log.(x)
plot(x, ỹ)
plot!(x, y)

# parametric-splines
t = 0:0.1:1
x = sin.(2pi*t)
y = cos.(2pi*t)
A = hcat(x,y)

itp = Interpolations.scale(interpolate(A, (BSpline(Cubic(Natural(OnGrid()))), NoInterp())), t, 1:2)
tfine = 0:0.01:1
xs, ys = [itp(t, 1) for t in tfine], [itp(t,2) for t in tfine]

scatter(x, y, label="knots")
plot!(xs, ys, label="spline")

itp1 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Natural(OnCell()))), NoInterp())), t, 1:2)
xs1, ys1 = [itp1(t, 1) for t in tfine], [itp1(t,2) for t in tfine]
plot!(xs1, ys1, label="spline2")

itp2 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Periodic(OnCell()))), NoInterp())), t, 1:2)
xs2, ys2 = [itp2(t, 1) for t in tfine], [itp2(t,2) for t in tfine]
plot!(xs2, ys2, label="spline3")

# itp3 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Free(OnCell()))), NoInterp())), t, 1:2)
# xs3, ys3 = [itp3(t, 1) for t in tfine], [itp3(t,2) for t in tfine]
# plot!(xs3, ys3, label="spline3")
#
# itp4 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Free(OnGrid()))), NoInterp())), t, 1:2)
# xs4, ys4 = [itp4(t, 1) for t in tfine], [itp4(t,2) for t in tfine]
# plot!(xs4, ys4, label="spline4")
#
# itp5 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Reflect(OnGrid()))), NoInterp())), t, 1:2)
# xs5, ys5 = [itp5(t, 1) for t in tfine], [itp5(t,2) for t in tfine]
# plot!(xs5, ys5, label="spline5")

# parametric-splines 2
t = [0.0, 1/6, 1/6, 2/6, 3/6, 3/6, 4/6, 5/6, 5/6, 1.0]
x = [0.0, -2.0, -2.0, -3.0, -4.0, -4.0, 0.25, 4.5, 4.5, 0.0]
y = [0.0, -2.0, -2.0, 0.0, -2.0, -2.0, 0.0, -2.0, -2.0, 0.0]
A = hcat(x,y)

itp = Interpolations.scale(interpolate(A, (BSpline(Cubic(Natural(OnGrid()))), NoInterp())), t, 1:2)
tfine = 0:0.01:1
xs, ys = [itp(t, 1) for t in tfine], [itp(t,2) for t in tfine]

scatter(x, y, label="knots")
plot!(xs, ys, label="spline")

itp1 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Natural(OnCell()))), NoInterp())), t, 1:2)
xs1, ys1 = [itp1(t, 1) for t in tfine], [itp1(t,2) for t in tfine]
plot!(xs1, ys1, label="spline2")

itp2 = Interpolations.scale(interpolate(A, (BSpline(Cubic(Periodic(OnCell()))), NoInterp())), t, 1:2)
xs2, ys2 = [itp2(t, 1) for t in tfine], [itp2(t,2) for t in tfine]
plot!(xs2, ys2, label="spline3")

using BSplines

basis = BSplines.BSplineBasis(4, 0:5)
plot(basis)
spl = BSplines.approximate(sin, basis)
plot(1:5, spl)
plot(spl, 1,3)
plot(spl)

f1 = Function(spl)
f2 = Function(spl, Derivative(1))
f3 = Function(spl, Derivative(2))
plot([f1, f2, f3])

basis2 = BSplineBasis(3, [1, 2, 3, 4, 5, 5, 6, 7, 8])
plot(basis2)
BSplines.knots(basis2)
b = bsplines(basis2, 3.5)

# splines consist of b-spline basis and a coefficient vector of the same length
t = [0, 1, 2, 3, 4, 5]
points = [0 0; 1 0; 1 1; 0 1; 0 2; 1 2];
x = points[:,1]
y = points[:,2]
plot(t,x); scatter!(t,x)
plot(t,y); scatter!(t,y)
plot(x,y); scatter!(x,y)

basis = averagebasis(3, t)
xspline = BSplines.interpolate(basis, t, x)
plot(t, xspline); scatter!(t, x)
yspline = BSplines.interpolate(basis, t, y)
plot(t, yspline); scatter!(t, y)
xapprox = xspline.(range(0, 5.0; length=100))
yapprox = yspline.(range(0.0, 5.0; length=100))
plot(xapprox, yapprox); scatter!(x,y)

# 3-d splines
t = [0, 1, 2, 3, 4, 5]
points = [0 0 0; 1 0 0; 1 1 0; 0 1 0; 0 2 0; 1 2 0];
x = points[:,1]
y = points[:,2]
z = points[:,3]
plot(t,x); scatter!(t,x)
plot(t,y); scatter!(t,y)
plot(t,z); scatter!(t,z)
plot3d(x,y,z); scatter3d!(x,y,z)

basis = averagebasis(3,t)
xspline = BSplines.interpolate(basis, t, x)
plot(xspline); scatter!(t,x)
yspline = BSplines.interpolate(basis, t, y)
plot(yspline); scatter!(t,y)
zspline = BSplines.interpolate(basis, t, z)
plot(zspline); scatter!(t,z)
xapprox = xspline.(range(0, 5.0; length=100))
yapprox = yspline.(range(0.0, 5.0; length=100))
zapprox = zspline.(range(0.0, 5.0; length=100))
plot3d(xapprox, yapprox, zapprox)
scatter3d!(x,y,z)

# get the derivatives
xdis = Function(xspline)
xvel = Function(xspline, Derivative(1))
xacc = Function(xspline, Derivative(2))
plot([xdis, xvel, xacc], labels=["disp","vel", "acc"])

ydis = Function(yspline)
yvel = Function(yspline, Derivative(1))
yacc = Function(yspline, Derivative(2))
# yjerk = Function(yspline, Derivative(3))
plot([ydis, yvel, yacc], labels=["disp","vel", "acc"])

zdis = Function(zspline)
zvel = Function(zspline, Derivative(1))
zacc = Function(zspline, Derivative(2))
plot([zdis, zvel, zacc], labels=["disp","vel", "acc"])

# try a 3-d path
t = [0, 1, 2, 3, 4]
points = [0 0 0; 0 0.5 -2; 1 1 0; 2 1.5 -2; 0 0 0];
x = points[:,1]
y = points[:,2]
z = points[:,3]
plot(t,x); scatter!(t,x)
plot(t,y); scatter!(t,y)
plot(t,z); scatter!(t,z)
plot3d(x,y,z); scatter3d!(x,y,z)

basis = averagebasis(3,t)
xspline = BSplines.interpolate(basis, t, x)
plot(xspline); scatter!(t,x)
yspline = BSplines.interpolate(basis, t, y)
plot(yspline); scatter!(t,y)
zspline = BSplines.interpolate(basis, t, z)
plot(zspline); scatter!(t,z)
xapprox = xspline.(range(0, 5.0; length=100))
yapprox = yspline.(range(0.0, 5.0; length=100))
zapprox = zspline.(range(0.0, 5.0; length=100))
plot3d(xapprox, yapprox, zapprox)
scatter3d!(x,y,z)

# try another basis with stops at -2 points
t = [0, 1, 2, 3, 4]
points = [0 0 0; 0 0.5 -2; 1 1 0; 2 1.5 -2; 0 0 0];
x = points[:,1]
y = points[:,2]
z = points[:,3]
plotly()
p1 = plot(t,x, label="x"); scatter!(t,x, label=:none)
p2 = plot(t,y, label="y", linecolor=:green); scatter!(t,y, label=:none)
p3 = plot(t,z, label="z", linecolor=:purple); scatter!(t,z, label="Via Points")
plt = plot(p1, p2, p3, layout=(3,1), legend=:outertopright)
p4 = plot3d(x,y,z, xlabel="x", ylabel="y", zlabel="z", label=:none); scatter3d!(x,y,z, markersize=2, label=:none)
plt2 = plot(plt, p4, layout=(1,2))


plotly()
basis = averagebasis(3, t)
basis = BSplines.BSplineBasis(4, [0.0, 1.0, 1.0, 2.0, 3.0, 3.0, 4.0]) # BEAUTIFUL!
# basis = averagebasis(4, t)
# basis = BSplines.BSplineBasis(3, [0.0, 0.0, 1.0, 1.0, 1.5, 2.5, 4.0, 4.0])
# basis = BSplines.BSplineBasis(4, [0.0, 0.0, 1.0, 1.0, 3.0, 3.0, 4.0, 4.0]) # BEAUTIFUL!
# basis = BSplines.BSplineBasis(4, [0.0, 0.0, 1.0, 3.0, 4.0, 4.0]) # BEAUTIFUL!
xspline = BSplines.interpolate(basis, t, x)
p1 = plot(xspline, label="x"); scatter!(t,x, label=:none)
yspline = BSplines.interpolate(basis, t, y)
p2 = plot(yspline, label="y", linecolor=:green); scatter!(t,y, label=:none)
zspline = BSplines.interpolate(basis, t, z)
p3 = plot(zspline, label="z", linecolor=:purple); scatter!(t,z, label="Via Points")
plt = plot(p1, p2, p3, layout=(3,1), legend=:outertopright)
xapprox = xspline.(range(0.0, 4.0; length=401))
yapprox = yspline.(range(0.0, 4.0; length=401))
zapprox = zspline.(range(0.0, 4.0; length=401))
p4 = plot3d(xapprox, yapprox, zapprox, xlabel="x", ylabel="y", zlabel="z", label=:none); scatter3d!(x,y,z, markersize=2, label=:none)
plt2 = plot(plt, p4, layout=(1,2))

plot(basis, legend=:none, title="Trajectory Basis Functions")

# get the derivatives
xdis = Function(xspline)
xvel = Function(xspline, Derivative(1))
xacc = Function(xspline, Derivative(2))

ydis = Function(yspline)
yvel = Function(yspline, Derivative(1))
yacc = Function(yspline, Derivative(2))

zdis = Function(zspline)
zvel = Function(zspline, Derivative(1))
zacc = Function(zspline, Derivative(2))
