'// ===== 1 ======= 2 ======= 3 ======= 4 ======= 5 ======= 6 ======= 7 ======= 8 ======= 9 ======= 10
' Source code for JustBasic

nomainwin
    WindowWidth = 800
    WindowHeight  = 600
    open "Natural Cubic Spline Solver: Tor Olav Kristensen, Christian Nevado, Bald Eagle" for graphics_nf_nsb as #grid
    #grid, "trapclose [quit]"

Size = 5
N = Size - 1

dim TT(Size)
dim XX(Size)
dim YY(Size)
dim ZZ(Size)

TT(0) = -2
XX(0) = -2
YY(0) = 0
ZZ(0) = 40

TT(1) = -1
XX(1) = 0
YY(1) = 0
ZZ(1) = -2

TT(2) = 0
XX(2) = 1
YY(2) = 0
ZZ(2) = 2

TT(3) = 0.5
XX(3) = 0
YY(3) = 6
ZZ(3) = 2

TT(4) = 1
XX(4) = 8
YY(4) = 2
ZZ(4) = 2


MinT = TT(0)
MaxT = TT(0)
MinX = XX(0)
MaxX = XX(0)
MinY = YY(0)
MaxY = YY(0)
MinZ = ZZ(0)
MaxZ = ZZ(0)

for i = 1 to N
   MinT = min(MinT, TT(i))
   MaxT = max(MaxT, TT(i))

   MinX = min(MinX, XX(i))
   MaxX = max(MaxX, XX(i))

   MinY = min(MinY, YY(i))
   MaxY = max(MaxY, YY(i))

   MinZ = min(MinZ, ZZ(i))
   MaxZ = max(MaxZ, ZZ(i))
next i

MinXYZ = min(MinX, min(MinY, MinZ))
MaxXYZ = max(MaxX, max(MaxY, MaxZ))

SpanT = MaxT - MinT
SpanX = MaxX - MinX
SpanY = MaxY - MinY
SpanZ = MaxZ - MinZ
SpanXYZ = MaxXYZ - MinXYZ


dim tt(Size)
dim yy(Size)

dim hh(Size)
dim uu(Size)
dim vv(Size)
dim zz(Size)

dim kk(Size, 3)


'// Copy TT to tt
for i = 0 to N
    tt(i) = TT(i)
next i

'// Calculate coefficients for the X polynomials

'// Copy XX to yy
for i = 0 to N
    yy(i) = XX(i)
next i

call CalculateCoefficients N

dim CoeffsX(Size, 3)

'Copy kk to CoeffsX
for i = 0 to N-1
    CoeffsX(i, 0) = kk (i, 0)
    CoeffsX(i, 1) = kk (i, 1)
    CoeffsX(i, 2) = kk (i, 2)
next i

'// Calculate coefficients for the Y polynomials

'// Copy YY to yy
for i = 0 to N
    yy(i) = YY(i)
next i

call CalculateCoefficients N

dim CoeffsY(Size, 3)

'// Copy kk to CoeffsY
for i = 0 to N-1
    CoeffsY(i, 0) = kk(i, 0)
    CoeffsY(i, 1) = kk(i, 1)
    CoeffsY(i, 2) = kk(i, 2)
next i

'// Calculate coefficients for the Z polynomials

'// Copy ZZ to yy
for i = 0 to N
    yy(i) = ZZ(i)
next i

call CalculateCoefficients N

dim CoeffsZ(Size, 3)

'// Copy kk to CoeffsZ
for i = 0 to N-1
    CoeffsZ(i, 0) = kk(i, 0)
    CoeffsZ(i, 1) = kk(i, 1)
    CoeffsZ(i, 2) = kk(i, 2)
next i


Intervals = 400

Tstart = TT(0)
Tend = TT(N)
Tspan  = Tend - Tstart

for j = 0 to Intervals - 1
    T = Tstart + j/Intervals*Tspan
    X = 0
    Y = 0
    Z = 0

    if T < TT(0) then
        i = 0
        call CalculateCoordinates i, T, X, Y, Z
    end if

    for  i = 0 to N - 1
        if TT(i) <= T and T < TT(i+1) then
            call CalculateCoordinates i, T, X, Y, Z
        end if
    next i

    if TT(N) <= T then
        i = N - 1
        call CalculateCoordinates i, T, X, Y, Z
    end if

    AbscissaT = Abscissa(T, MinT, MaxT)
    Ordinate0 = Ordinate(0, MinXYZ, MaxXYZ)
    OrdinateX = Ordinate(X, MinXYZ, MaxXYZ)
    OrdinateY = Ordinate(Y, MinXYZ, MaxXYZ)
    OrdinateZ = Ordinate(Z, MinXYZ, MaxXYZ)

    #grid, "size 2"
    #grid, "down"
    #grid, "color black"
    #grid, "set "; AbscissaT; " "; Ordinate0
    #grid, "color red"
    #grid, "set "; AbscissaT; " "; OrdinateX
    #grid, "color green"
    #grid, "set "; AbscissaT; " "; OrdinateY
    #grid, "color blue"
    #grid, "set "; AbscissaT; " "; OrdinateZ
    #grid, "up"
next j

'// ===== 1 ======= 2 ======= 3 ======= 4 ======= 5 ======= 6 ======= 7 ======= 8 ======= 9 ======= 10

wait

'// ===== 1 ======= 2 ======= 3 ======= 4 ======= 5 ======= 6 ======= 7 ======= 8 ======= 9 ======= 10

function min(A, B)

    min = A
    if B < A then min = B

end function


function max(A, B)

    max = A
    if B > A then max = B

end function


function Abscissa(T, MinT, MaxT)

    Abscissa = (T - MinT)/(MaxT - MinT)*WindowWidth

end function


function Ordinate(V, MinV, MaxV)

    Ordinate = (1 - (V - MinV)/(MaxV - MinV))*WindowHeight/2

end function


function CubicPolynomial(T, A, B, C, D)

    CubicPolynomial = (D + T*(C + T*(B + T*A))) '// Equals (A*T^3 + B*T^2 + C*T + D)

end function


sub CalculateCoordinates i, T, byref X, byref Y, byref Z

    dt = T - TT(i)
    X = CubicPolynomial(dt, CoeffsX(i, 0), CoeffsX(i, 1), CoeffsX(i, 2), XX(i))
    Y = CubicPolynomial(dt, CoeffsY(i, 0), CoeffsY(i, 1), CoeffsY(i, 2), YY(i))
    Z = CubicPolynomial(dt, CoeffsZ(i, 0), CoeffsZ(i, 1), CoeffsZ(i, 2), ZZ(i))

end sub


sub CalculateCoefficients N

    for i = 0 to N - 1
        hh(i) = tt(i+1) - tt(i)
        kk(i, 1) = 6*(yy(i+1) - yy(i))/hh(i)
    next i

    uu(1) = 2*(hh(0) + hh(1))
    vv(1) = kk(1, 1) - kk(0, 1)
    for i = 2 to N - 1
        uu(i) = 2*(hh(i) + hh(i-1)) - hh(i-1)^2/uu(i-1)
        vv(i) = kk(i, 1) - kk(i-1, 1) - hh(i-1)*vv(i-1)/uu(i-1)
    next i

    zz(N) = 0
    for i = N - 1 to 1 step -1
        zz(i) = (vv(i) - hh(i)*zz(i+1))/uu(i)
    next i
    zz(0) = 0

    for i = 0 to N - 1
        kk(i, 0) = (zz(i+1) - zz(i))/(6*hh(i))
        kk(i, 1) = zz(i)/2
        kk(i, 2) = (yy(i+1) - yy(i))/hh(i) - hh(i)/6*(zz(i+1) + 2*zz(i))
    next i

end sub

'// ===== 1 ======= 2 ======= 3 ======= 4 ======= 5 ======= 6 ======= 7 ======= 8 ======= 9 ======= 10

[quit]
    close #grid
    end

'// ===== 1 ======= 2 ======= 3 ======= 4 ======= 5 ======= 6 ======= 7 ======= 8 ======= 9 ======= 10