using Plots
using DSP
using CSV

pyplot()

# Parameters
filename = joinpath(@__DIR__, "data", "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),
    (title="big_quadra", order=2, sizes=11:10:51)
]


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


filter = savgol(19, 2, 1, h, true)
H = tf(filter, [1], h)
mag, phase, w = bode(H)
plot(w,log.(mag[:,1]), label="mag", title="plop")
savefig(joinpath(@__DIR__, "results", string("savgol_bode_", "plop", ".eps")))
show()
# for order in orders
#     for size in order.sizes
#         filter = savgol(size, order.order, 1, h, true)
#         H = tf(filter, [1], 0.05)
#         bode(H)
#     end
#     savefig(joinpath(@__DIR__, "results", string("savgol_bode_", order.title, ".eps")))
# end
# show()