using Plots
using DSP
using CSV

pyplot()

# Parameters
filename = "walk.csv" # input file
h = 1/22 # sample time
orders = [
    (title="quadratique", order=2, sizes=5:2:11),
    (title="cubique", order=3, sizes=5:2:11)
]


function savgol(size::Int64, poly_order::Int64, deriv::Int64=0, delta::Float64=1.0, conv::Bool=false)
    half_size, rem = divrem(size, 2)
    if rem == 0
        throw(ArgumentError("size must be odd."))
    end
    M = [-half_size:half_size;] .^ [0:poly_order;]';
    y = zeros(poly_order+1)';
    y[deriv+1] = factorial(deriv) / delta^deriv;
    scal = y*inv(M'*M)*M'
    if conv
        scal = scal[end:-1:1]
    end
    scal
end

error = (CSV.read(filename, header=false) |> Matrix{Float64})[:,2]


for order in orders
    plot(
        0:h:(length(error)-1)*h,
        error,
        label="erreur mesurée",
        layout=(2,1),
        subplot=1,
        title="Position",
        xrotation=60,
        xlabel="Temps (s)",
        ylabel="Position (m)",
        reuse=false,
        size=(1000, 600)
    )
    plot!(
        subplot=2,
        title="Vitesse mesurée",
        xrotation=60,
        xlabel="Temps (s)",
        ylabel="Vitesse (\$m.s^{-1}\$)"
    )
    for size in order.sizes
        filter = savgol(size, order.order, 1, h, true)
        speed = conv(filter, error)[size:end-size]
        plot!(
            size*h/2:h:size*h/2+(length(speed)-1)*h,
            speed,
            label=string(order.title, " ", size, " points"),
            subplot=2
        )
    end
    savefig(string("mesure_vitesse_", order.title, ".eps"))
end
show()