function resample(s, w)
    # Samples new particles based on their weights. If you find algorithmic optimizations to this routine, please tell me /Bagge)
    N = length(w)
    bins = [0.; cumsum(exp(w))]
    j = zeros(Int64,N)
    bo = 1
    for i = 1:N
        for b = bo:N
            if bins[b] <= s[i]# < bins[b+1]
                j[i] = b
                bo = b
                break
            end
        end
    end
    return j
end

function resample(w)
    bins = [0.; cumsum(exp(w))]
    s = collect((rand()/N+0):1/N:bins[end])
    resample(s,w)
end

@doc """ `xhat = SQMC(f, g, ginv, x0, y, est, N=200, Nu=lenth(x0))`

    Gives estimation of the states `x(:,1),...x(:,T)` given outputs `y(:,1)...y(:,T)` from system:
    
    x(t) = f(x(t-1),t,u) \n
    y = gn(x,v),\n
    where `g(x,t)=E[gn(x,t,u)]`, `ginv(yhat, y, t) = p(y=gn(x,u)|yhat=g(x))`,
    u is uniform noise on `[0,1)`, with `E[x(0)] = xhat0`,
    and some estimator `est(xₚ,w,t)` that ouputs the estimate `xhat(:,t)`
    given the particles `xₚ` and weights `w`
    """ ->
function SQMC(f!, g, ginv, xhat0, y, est, N=200, Nu = length(xhat0); debug = false, xreal=xreal)
    T = size(y,2)
    #Estimates at time 1:T
    xhat = Array{Float64,2}(Nu, T)
    #Current predicted output for each particle
    yhat = Array{Float64,2}(size(y,1), N)
    x = Array{Float64,2}(length(xhat0), N)
    xtemp = similar(x)
    w = Array{Float64,1}(N)
    #Step (a) (Draw u and x), OBS u will be of size NxNu
    u, = sobol(Nu,N)
    x[:,1] = xhat0; #Hack to use the pfStep fuction. Works by letting `a` be ones
    pfStep!(f!,g,ginv,x,xtemp,u,ones(Int,N),1:N,1,w,yhat,y,N, 1)
    xhat[:,1] = est(x,w,1)
    u, nextseed, MeM = sobol(Nu+1,N)
    for t = 2:T
        nextseed = sobol!(u, nextseed, MeM)
        τ = sortperm(u[:,1])
        σ = sortperm(ψ(x)[:])
        a = resample(u[τ,1], w[σ])
        # Time update, no sigma here?
        pfStep!(f!,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N, 2:Nu+1)
        xhat[:,t] = est(x,w,1)
        if debug
            plotPoints(x, w, y, N, a, τ, t, xreal, xhat)
        end
    end
    xhat
end


function ψ(x)
    x
end

function pf(f!, g, ginv, xhat0, y, est, N=200, Nu = length(xhat0); debug=false, xreal=xreal)
    T = size(y,2)
    #Estimates at time 1:T
    xhat = Array{Float64,2}(Nu, T)
    #Current predicted output for each particle
    yhat = Array{Float64,2}(size(y,1), N)
    x = Array{Float64,2}(length(xhat0), N)
    xtemp = similar(x)
    w = Array{Float64,1}(N)
    τ = 1:N
    u = rand(N,Nu)
    x[:,1] = xhat0; #Hack to use the pfStep fuction. Works by letting `a` be ones
    pfStep!(f!,g,ginv,x,xtemp,u,ones(Int,N),τ,1,w,yhat,y,N, 1:Nu)
    xhat[:,1] = est(x,w,1)
    for t = 2:T
        u = rand(N,Nu)
        a = resample(u[τ,1], w[:])
        # Time update, no sigma here?
        pfStep!(f!,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N,1:Nu)
        xhat[:,t] = est(x,w,1)
    end
    xhat
end

#Time update and weighting. Chances `x`, `yhat` and `w`
@inbounds function pfStep!(f,g,ginv,x,xtemp,u,a,τ,t,w,yhat,y,N,uRange)
    for i = 1:N
        f(x[:,a[i]], t, u[τ[i], uRange], xtemp, i)
        yhat[:,i] = g(xtemp[:,i], t)
        w[i] = ginv(yhat[:,i], y[:,t], t)
    end
    x[:] = copy(xtemp)
    
    offset = maximum(w)
    mySum = sum(exp(w-offset))
    normConstant = log(mySum)+offset
    w[:] = w[:] - normConstant
end