using LinearAlgebra
using NumericalIntegration
using ProgressBars


include("Quaternions.jl")

dt = 0.005
time = 0.0:dt:60

I = [3 0 0; 0 4 0; 0 0 2]
Iinv = inv(I)
T = [0; 0; 0]
ω = [2.001; 0.001; 0.001]
ω′ = [0; 0; 0]

q = Quaternion([0; 0; 0; 1])

ω_last = [0; 0; 0; ω]
ω0 = copy(ω_last)
ω_area = [0; 0; 0; 0; 0; 0]

β = [0; 0; 0]

integrate_ω(T, I, ω) = Iinv * (T - cross(ω, I * ω))

function q_step(β)
    mag = β .^ 2 |> sum |> sqrt

    Quaternion([normalize(β) .* sin(mag / 2); cos(mag / 2)])

end

function integrate_vector(v, v_last)
    v_new = similar(v)
    for i = 1:length(v)
        v_new[i] = integrate([0, dt], [v_last[i], v[i]])
    end
    return v_new
end


qs = []
for t in ProgressBar(time)
    # t = collect(time)[1]
    ω′ = integrate_ω(T, I, ω)


    ω_new = integrate_vector([ω; ω′], ω_last)
    β = ω_new[1:3] .- β

    ω_last = [ω; ω′]
    ω = ω_new[4:6] .- ω
    ω_area = ω_new .+ ω_area

    q = q * q_step(β)

    push!(qs, q)

end



# Plot 
using Plots
let
    plot(title = "T Handle Quaternion")
    plot!([q[1] for q in qs], label = "i")
    plot!([q[2] for q in qs], label = "j")
    plot!([q[3] for q in qs], label = "k")
    plot!([q[4] for q in qs], label = "r")
end
# println("done")

# plot(qs)

# Write to CSV
# using CSV
# using DataFrames

# ts = 100
# df = DataFrame(
#     time = time[1:ts:end],
#     yaw = yaw[1:ts:end],
#     roll = roll[1:ts:end],
#     pitch = pitch[1:ts:end],
# )

# CSV.write("ypr.csv", df, header = false)
# df