text:
include("MultiPathGP.jl")

using Printf

function TestLearning(n = 100, method = "full")
    t = range(0, stop = 100, length = n)
    D = zeros(n, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    gp = LearnDeviationFunction(D, true, method)
    plot(
        legend = false,
        xlabel = "Prototypical Speed [θ/s]",
        ylabel = "Deviation Function [θ/s]",
        title = "Deviation Function Learning Performance",
    )
    t = range(0, stop = 20, length = 1000)
    plot!(t, Deviation.(t), linestyle = :dash, color = :red, linealpha = 0.5)
    p = plot!(gp, ylims = (-2, 3), xlims = (4, 12), markeralpha = 0.6)
    display(p)
    savefig("gp_perf.png")
end

function CompareMethods(n = 100)
    t = range(0, stop = 100, length = n)
    D = zeros(n, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    gp = LearnDeviationFunction(D, true, "full")
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    gp2 = LearnDeviationFunction(D, true, "DTC")
    plot(
        legend = :topleft,
        xlabel = L"\textrm{Prototypical Speed }[\theta/s]",
        ylabel = L"\textrm{Deviation Function }[\theta/s]",
        title = L"\textrm{Fitting Performance, DTC vs Full GP}",
        formatter = :latex,
    )
    t = range(0, stop = 20, length = 1000)
    p = plot!(
        gp,
        ylims = (-2, 3),
        xlims = (4, 12),
        markeralpha = 0.2,
        markercolor = :black,
        linecolor = :black,
        fillcolor = :black,
        fillalpha = 0.2,
        label = [L"\textrm{Full GP}" ""],
        formatter = :latex,
    )
    p = plot!(
        gp2,
        ylims = (-2, 3),
        xlims = (4, 12),
        markeralpha = 0.2,
        markercolor = :green,
        linecolor = :green,
        fillcolor = :green,
        fillalpha = 0.2,
        label = [L"\textrm{DTC GP}" ""],
        formatter = :latex,
    )
    plot!(
        t,
        Deviation.(t),
        linestyle = :dash,
        color = :red,
        linealpha = 0.5,
        label = L"\textrm{True Deviation}",
        formatter = :latex,
    )
    display(p)
    savefig("gp_comp.pdf")
end

function BenchmarkLearning(n = 100)
    t = range(0, stop = 100, length = n)
    D = zeros(n, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    @benchmark LearnDeviationFunction($D, $true, "full")
    @benchmark LearnDeviationFunction($D, $true, "SOR")
    @benchmark LearnDeviationFunction($D, $true, "DTC")
end

function AnimateLearning(n = 100)
    t = range(0, stop = 100, length = n)
    D = zeros(n, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    anim = @animate for i = 1:n
        @show i
        k = i <= 5 ? 5 : i
        gp = LearnDeviationFunction(D[1:k, :], true, "full")
        plot(
            gp,
            ylims = (-2, 6),
            xlims = (0, 16),
            legend = false,
            xlabel = "Prototypical Speed [θ/s]",
            ylabel = "Deviation Function [θ/s]",
            markeralpha = 0.3,
        )
    end
    gif(anim, "Learning_Anim.gif", fps = 30)
end

function PlotPosPrediction(n = 100, t0 = 20.0, tf = 100.0)
    rng = MersenneTwister(1234)
    t = range(0, stop = t0, length = n) #just learn the first t0s
    D = zeros(n, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(n)
    gp = LearnDeviationFunction(D, true, "DTC")
    #plot(gp, ylims = (-2, 10), xlims = (0, 16))
    t = range(0.0, stop = tf, length = 100)
    p = zeros(length(t))
    s = zeros(length(t))
    v = zeros(length(t))
    vp = VelocityPrior.(t)
    #@benchmark DriverPosition($t[1], $t[2], $gp, 1e-1)
    Threads.@threads for i = 1:length(t)
        p[i] = DriverPosition(t[1], t[i], gp)
        s[i] = DriverUncertainty(t[1], t[i], gp, 1e-3)
        v[i] = DriverVelocity(t[i])
    end
    p1 = plot(
        t,
        v,
        title = L"\textrm{True Velocity}",
        ylabel = L"\textrm{Speed }[\theta/s]",
        xlabel = L"\textrm{Time }[s]",
        label = L"\dot{\theta^d}",
        legend = :bottomright,
        xlims = (0, 100),
        ylims = (5, 12),
        formatter = :latex,
    )
    p1 = plot!(t, vp, label = L"\dot{\theta^h}")
    minv = minimum(D[:, 1])
    maxv = maximum(D[:, 1])
    p1 = plot!(
        rectangle(1000, maxv - minv, -100, minv),
        opacity = 0.2,
        label = L"\mathcal{O}",
        formatter = :latex,
    )
    p2 = plot(
        t,
        s,
        title = L"\textrm{Position Uncertainty}",
        ylabel = L"\Sigma_d [\theta]",
        xlabel = L"\textrm{Time }[s]",
        legend = false,
        ylims = (0, 35),
        formatter = :latex,
    )
    p3 = plot(
        gp,
        ylims = 1 .* (-2, 3),
        xlims = (4, 12),
        title = L"\textrm{Gaussian Process}",
        xlabel = L"\dot{\theta^h}\thinspace[\theta/s]",
        ylabel = L"d\thinspace [\theta/s]",
        legend = false,
        formatter = :latex,
    )
    p3 = plot!(
        range(0, stop = 20, length = 1000),
        Deviation.(range(0, stop = 20, length = 1000)),
        linestyle = :dash,
        color = :red,
        linealpha = 0.5,
        label = "True Deviation",
        formatter = :latex,
    )
    l = @layout [a b; c{0.5h}]
    p = plot(p1, p2, p3, layout = l)
    display(p)
    savefig("learning_ex.pdf")
end

function fstar(x::Float64)
    return abs(x - 5) * cos(2 * x)
end

function benchmarkGP()

    n = 50:50:300

    times = zeros(length(n), 2)
    stds = copy(times)
    σy = 10.0
    for i = 1:length(n)
        @show i, n[i]
        Random.seed!(1) # for reproducibility
        Xdistr = Beta(7, 7)
        ϵdistr = Normal(0, σy)
        x = rand(Xdistr, n[i]) * 10
        X = Matrix(x')
        Y = fstar.(x) .+ rand(ϵdistr, n[i])
        k = SEIso(log(0.3), log(5.0))
        Xu = Matrix(
            quantile(
                x,
                [
                    0.2,
                    0.25,
                    0.3,
                    0.35,
                    0.4,
                    0.45,
                    0.5,
                    0.55,
                    0.6,
                    0.65,
                    0.7,
                    0.98,
                ],
            )',
        )
        GPE(X, Y, MeanConst(mean(Y)), k, log(σy))
        t = @benchmark gp_full = GPE($X, $Y, $MeanConst(mean(Y)), $k, $log(σy))
        @show t
        t = t.:times
        t[t.>=3*median(t)] .= median(t)
        times[i, 1] = median(t)
        stds[i, 1] = std(t)
        gp_DTC = GaussianProcesses.DTC(X, Xu, Y, MeanConst(mean(Y)), k, log(σy))
        t = @benchmark gp_DTC = GaussianProcesses.DTC(
            $X,
            $Xu,
            $Y,
            $MeanConst(mean(Y)),
            $k,
            $log(σy),
        )
        @show t
        t = t.:times
        t[t.>=5*median(t)] .= median(t)
        times[i, 2] = median(t)
        stds[i, 2] = std(t)
    end
    p = plot(
        n,
        times[:, 1],
        yerror = stds[:, 1],
        label = L"\textrm{Full GP}",
        legend = :topleft,
        formatter = :latex,
    )
    p = plot!(n, times[:, 2], yerror = stds[:, 2], label = L"\textrm{DTC GP}")
    p = xlabel!(L"N")
    p = ylabel!(L"\textrm{CPU Time }[ns]")
    p = title!(L"\textrm{GPR Computation Times}")
    display(p)
    savefig("gpr_times.pdf")
    return times, stds, n
end

function CEconvergence(ntrajs = 5, niter = 5)
    UASPos = [800, 450]
    LPos = [600, 600]
    ts = 0
    PNRStat = false
    np = [1, 2]
    ptgt = 2
    dt = 1
    N = 100
    Ns = 5
    PosSamples = zeros(N, length(p))
    OptTimeSample = zeros(length(p))
    gp = nothing
    resv = zeros(ntrajs, niter)
    Threads.@threads for j = 1:ntrajs
        μ = 60 * ones(2) .+ 2 * randn(2)
        Σ = 10 * ones(2) .+ 1 * randn(2)
        for i = 1:niter
            #sample rendezvous candidates
            UASPos = [800, 450] .+ 50 * randn(2)
            TimeSamples = SampleTime(μ, Σ, N)
            elites, ptgt =
                rankMultiPath(TimeSamples, UASPos, Ns, np, gp, 0.0, 0.0)
            CEM(μ, Σ, np, elites, OptTimeSample, TimeSamples)
            resv[j, i] = Σ[1]
        end
        @show j
    end
    pp = plot(
        1:niter,
        resv',
        linealpha = 0.1,
        lw = 2,
        label = "",
        linecolor = :green,
        ylims = (0, 6),
    )
    resm = mean(resv, dims = 1)'
    pp = plot!(
        1:niter,
        resm,
        lw = 3,
        linecolor = :black,
        label = L"\textrm{Mean of Trials}",
    )
    pp = hline!(
        [1.0],
        linestyle = :dash,
        linecolor = :red,
        formatter = :latex,
        title = L"\textrm{Importance Sampling Rate of Convergence}",
        xlabel = L"\textrm{Iteration}",
        ylabel = L"\Sigma_\mathcal{A}",
        label = L"\lambda",
    )
    display(pp)
    savefig("ce_con.pdf")
    return resm, resv
end

function iterDriverAndCE(tt = 20, ti = 5, dt = 1)
    #first build an initial dataset and gp
    t = range(0.0, stop = ti, step = dt)
    li = length(t)
    D = zeros(li, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(li)
    gp = LearnDeviationFunction(D, true, "DTC")
    #now we move the driver, collect new data, update gp and sample on the gp
    t = range(0.0, stop = tt, step = dt)
    l = length(t)
    D = [D; zeros(l, 2)]
    PosSamples = zeros(N, length(p))
    OptTimeSample = zeros(length(p))
    μ = 60 * ones(2) .+ 2 * randn(2)
    Σ = 10 * ones(2) .+ 1 * randn(2)
    UASPos = [800, 450] .+ 50 * randn(2)
    DriverPos = 0.0
    Σv = zeros(l)
    for i = 1:l
        #sample driver velocity
        DriverVelSample = DriverVelocity(t[i]) + NoiseStd * randn(1)[1]
        DriverDeviationSample = DriverVelSample - VelocityPrior(t[i])
        #add to dataset
        D[li+i, 1] = VelocityPrior(t[i])
        D[li+i, 2] = DriverDeviationSample
        #re-train gp
        gp = LearnDeviationFunction(D[1:(li+i), :], true, "DTC")
        #sample rendezvous candidates
        TimeSamples = SampleTime(μ, Σ, N)
        elites, ptgt =
            rankMultiPath(TimeSamples, UASPos, Ns, [1, 2], gp, t[i], DriverPos)
        CEM(μ, Σ, [1, 2], elites, OptTimeSample, TimeSamples)
        PosSamples = DriverPosition(t[i], TimeSamples, gp) .+ DriverPos
        DriverPos = DriverPos + DriverVelocity(t[i]) * dt
        Σv[i] = Σ[1]
    end
    return Σv[1:end-1]
end

function CEconvergenceMdlUpdate(ntrajs = 5, niter = 50)
    #this variant updates driver model
    ts = 0
    PNRStat = false
    np = [1, 2]
    ptgt = 2
    dt = 1
    N = 100
    Ns = 5
    PosSamples = zeros(N, length(p))
    OptTimeSample = zeros(length(p))
    gp = nothing
    resv = zeros(ntrajs, niter)
    Threads.@threads for j = 1:ntrajs
        Σv = iterDriverAndCE(niter, 5, 1)
        resv[j, :] = Σv'
        @show j
    end
    resm = mean(resv, dims = 1)'
    pp1 = plot(
        1:niter,
        resv',
        linealpha = 0.1,
        lw = 2,
        label = "",
        linecolor = :green,
        formatter = :latex,
    )
    resm = mean(resv, dims = 1)'
    pp1 = plot!(
        1:niter,
        resm,
        lw = 3,
        linecolor = :black,
        label = L"\textrm{Mean of Trials}",
        formatter = :latex,
    )

    pp1 = hline!(
        [1.0],
        linestyle = :dash,
        linecolor = :red,
        formatter = :latex,
        title = L"\textrm{Importance Sampling Performance}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\Sigma_\mathcal{A}",
        label = L"\lambda",
        ylims = (0.75, 1.5),
        xlims = (0, 40),
        xticks = 0:4:40,
    )
    display(pp1)
    savefig("ce_mdl_up.pdf")
    return pp1
end

function riskcon(Er = 18000, rmethod = "WorstFirst", tt = 80, ti = 5, dt = 1)
    UASPos = [800, 450]
    LPos = [1000, 600]
    #LPos = [800, 450]
    ts = 0
    PNRStat = false
    N = 100
    p = [1, 2]
    ptgt = 2
    dt = 1
    PrevPNR = [0.0, 0.0]
    μ = 60 * ones(2)
    Σ = 10 * ones(2)
    Ns = 5
    UASPosVec = zeros(N, 2)
    #TimeSamples = ts:120
    t = range(0.0, stop = ti, step = dt)
    li = length(t)
    D = zeros(li, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(li)
    gp = LearnDeviationFunction(D, true, "DTC")
    #now we move the driver, collect new data, update gp and sample on the gp
    tv = range(0.0, stop = tt, step = dt)
    l = length(tv)
    D = [D; zeros(l, 2)]
    DriverPos = 0.0
    DriverPosVec = zeros(l)
    DriverPosEstimateVec = zeros(l)
    DriverPosErrorVec = zeros(l)

    Evr = zeros(l) #remaining
    Evd = zeros(l) #delivery
    Eva = zeros(l) #abort
    t1v = zeros(l) #t1
    dv = zeros(l)

    PosSamples = zeros(N, length(p))
    OptTimeSample = zeros(length(p))
    sol = nothing
    solr = 0.0
    riska = true

    riskv1 = zeros(l)
    riskv2 = copy(riskv1)
    iend = 0
    anim = @animate for i = 1:l
        iend = i
        #sample driver velocity
        DriverVelSample = DriverVelocity(tv[i]) + NoiseStd * randn(1)[1]
        DriverDeviationSample = DriverVelSample - VelocityPrior(tv[i])
        #add to dataset
        D[li+i, 1] = VelocityPrior(tv[i])
        D[li+i, 2] = DriverDeviationSample
        #re-train gp
        gp = LearnDeviationFunction(D[1:(li+i), :], true, "DTC")
        #sample rendezvous candidates
        UASPosVec[i, :] = UASPos
        TimeSamples = SampleTime(μ, Σ, N)
        elites, ptgt = rankMultiPath(
            TimeSamples,
            UASPos,
            Ns,
            p,
            gp,
            tv[i],
            DriverPos,
            rmethod,
            Er,
            sol,
            riska,
        )
        CEM(μ, Σ, p, elites, OptTimeSample, TimeSamples, Ns)
        PosSamples = DriverPosition(tv[i], TimeSamples, gp) .+ DriverPos
        #MPC goes here
        v, t = RendezvousPlanner(
            UASPos,
            LPos,
            OptTimeSample,
            Er,
            tv[i],
            PrevPNR,
            false,
            ptgt,
            gp,
            DriverPos,
        )
        sol = (v, t, LPos, UASPos, DriverPos, tv[i], OptTimeSample, ptgt)
        solr = sol
        pp = plot()
        pp = plot!(UASPosVec[1:i, 1], UASPosVec[1:i, 2], legend = false)
        pp = drawMultiConvexHull(PosSamples, p)
        pp = plotPlan!(
            UASPos,
            LPos,
            v,
            t,
            TimeSamples,
            length(p),
            tv[i],
            DriverPos,
            gp,
        )
        pp = plotTimeSamples!(TimeSamples, p, tv[i], DriverPos, gp)
        pp = scatter!(
            path(DriverPos, ptgt),
            markersize = 6.0,
            label = "",
            markercolor = :green,
        )
        s = @sprintf(
            "Distance %.2f",
            EuclideanDistance(UASPos, path(DriverPos, ptgt, true))
        )
        annotate!(200, 1000, text(s, 12, :left))
        EuclideanDistance(UASPos, path(DriverPos, ptgt, true))
        PrevPNR = v[:, 1] .* t[1] .+ UASPos
        Ed =
            m[1] * v[1, 1]^2 * t[1] +
            m[1] * v[1, 2]^2 * t[2] +
            m[2] * v[1, 3]^2 * t[3] +
            m[1] * v[2, 1]^2 * t[1] +
            m[1] * v[2, 2]^2 * t[2] +
            m[2] * v[2, 3]^2 * t[3] +
            m[1] * alpha * t[1] +
            m[1] * alpha * t[2] +
            m[2] * alpha * t[3]

        Evr[i] = Er
        Evd[i] = Ed
        Eva[i] =
            m[1] * v[1, 1]^2 * t[1] +
            m[1] * v[2, 1]^2 * t[1] +
            m[1] * v[1, 4]^2 * t[4] +
            m[1] * v[2, 1]^2 * t[4] +
            m[1] * alpha * t[1] +
            m[1] * alpha * t[4]

        x, y, Er = uav_dynamics(
            UASPos[1],
            UASPos[2],
            v[1, 1],
            v[2, 1],
            dt,
            Er,
            vmax,
            PNRStat ? m[2] : m[1],
        )
        UASPos = [x, y]
        if ptgt == 2
            pv = pv2
        else
            pv = pv1
        end
        # if (t[1] <= 1.0 ||
        #    EuclideanDistance(
        #     path(DriverPosition(tv[i], OptTimeSample[ptgt], gp) + DriverPos),
        #     pv[2, :],
        # ) <= 10) && tv[i] > 10
        #     break
        # end
        if (t[1] <= 1.0 && tv[i] > 10)
            break
        end
        #@show t, i, Σ, μ, Er, Ed
        DriverPosVec[i] = DriverPos
        DriverPosEstimateVec[i] = DriverPosition(0.0, tv[i])
        DriverPosErrorVec[i] = DriverPosVec[i] - DriverPosEstimateVec[i]
        dv[i] = EuclideanDistance(UASPos, path(DriverPos, ptgt, true))
        DriverPos = DriverPos + DriverVelocity(tv[i]) * dt

        vsq = rowNorm(sol[1])

        risk1, risk2, maintgt = GPCVaR(gp, solr, ptgt, 10000, 0.05)

        riskv1[i] = max(risk1, 0.0)
        riskv2[i] = max(risk2, 0.0)
        @show i, t[1], UASPos
    end
    #risk1 = GPCVaR(gp, solr, 1)
    #risk2 = GPCVaR(gp, solr, 2)
    #if (risk1 > 200 || risk2 > 200)
    #    println("Risk too high, aborting!")
    #end
    gif(anim, "RDV_Anim_Risk.gif", fps = 15)
    return riskv1, riskv2, iend, Evr, Evd, Eva, dv

end

function risktails(Er = 18000, rmethod = "WorstFirst", tt = 80, ti = 5, dt = 1)
    UASPos = [800, 450]
    LPos = [1000, 600]
    #LPos = [800, 450]
    ts = 0
    PNRStat = false
    N = 100
    p = [1, 2]
    ptgt = 2
    dt = 1
    PrevPNR = [0.0, 0.0]
    μ = 60 * ones(2)
    Σ = 10 * ones(2)
    Ns = 5
    UASPosVec = zeros(N, 2)
    #TimeSamples = ts:120
    t = range(0.0, stop = ti, step = dt)
    li = length(t)
    D = zeros(li, 2)
    D[:, 1] = VelocityPrior.(t)
    D[:, 2] = Deviation.(VelocityPrior.(t)) + NoiseStd .* randn(li)
    gp = LearnDeviationFunction(D, true, "DTC")
    #now we move the driver, collect new data, update gp and sample on the gp
    tv = range(0.0, stop = tt, step = dt)
    l = length(tv)
    D = [D; zeros(l, 2)]
    DriverPos = 0.0
    DriverPosVec = zeros(l)
    DriverPosEstimateVec = zeros(l)
    DriverPosErrorVec = zeros(l)

    Evr = zeros(l) #remaining
    Evd = zeros(l) #delivery
    Eva = zeros(l) #abort
    t1v = zeros(l) #t1
    dv = zeros(l)

    PosSamples = zeros(N, length(p))
    OptTimeSample = zeros(length(p))
    sol = nothing
    solr = 0.0
    riska = true

    riskv1 = zeros(l)
    riskv2 = copy(riskv1)
    iend = 0
    i = 1
    iend = i
    #sample driver velocity
    DriverVelSample = DriverVelocity(tv[i]) + NoiseStd * randn(1)[1]
    DriverDeviationSample = DriverVelSample - VelocityPrior(tv[i])
    #add to dataset
    D[li+i, 1] = VelocityPrior(tv[i])
    D[li+i, 2] = DriverDeviationSample
    #re-train gp
    gp = LearnDeviationFunction(D[1:(li+i), :], true, "DTC")
    #sample rendezvous candidates
    UASPosVec[i, :] = UASPos
    TimeSamples = SampleTime(μ, Σ, N)
    elites, ptgt = rankMultiPath(
        TimeSamples,
        UASPos,
        Ns,
        p,
        gp,
        tv[i],
        DriverPos,
        rmethod,
        Er,
        sol,
        riska,
    )
    CEM(μ, Σ, p, elites, OptTimeSample, TimeSamples, Ns)
    PosSamples = DriverPosition(tv[i], TimeSamples, gp) .+ DriverPos
    #MPC goes here
    v, t = RendezvousPlanner(
        UASPos,
        LPos,
        OptTimeSample,
        Er,
        tv[i],
        PrevPNR,
        false,
        ptgt,
        gp,
        DriverPos,
    )
    sol = (v, t, LPos, UASPos, DriverPos, tv[i], OptTimeSample, ptgt)
    solr = sol
    EuclideanDistance(UASPos, path(DriverPos, ptgt, true))
    PrevPNR = v[:, 1] .* t[1] .+ UASPos
    Ed =
        m[1] * v[1, 1]^2 * t[1] +
        m[1] * v[1, 2]^2 * t[2] +
        m[2] * v[1, 3]^2 * t[3] +
        m[1] * v[2, 1]^2 * t[1] +
        m[1] * v[2, 2]^2 * t[2] +
        m[2] * v[2, 3]^2 * t[3] +
        m[1] * alpha * t[1] +
        m[1] * alpha * t[2] +
        m[2] * alpha * t[3]

    Evr[i] = Er
    Evd[i] = Ed
    Eva[i] =
        m[1] * v[1, 1]^2 * t[1] +
        m[1] * v[2, 1]^2 * t[1] +
        m[1] * v[1, 4]^2 * t[4] +
        m[1] * v[2, 1]^2 * t[4] +
        m[1] * alpha * t[1] +
        m[1] * alpha * t[4]

    x, y, Er = uav_dynamics(
        UASPos[1],
        UASPos[2],
        v[1, 1],
        v[2, 1],
        dt,
        Er,
        vmax,
        PNRStat ? m[2] : m[1],
    )
    UASPos = [x, y]
    if ptgt == 2
        pv = pv2
    else
        pv = pv1
    end
    #@show t, i, Σ, μ, Er, Ed
    DriverPosVec[i] = DriverPos
    DriverPosEstimateVec[i] = DriverPosition(0.0, tv[i])
    DriverPosErrorVec[i] = DriverPosVec[i] - DriverPosEstimateVec[i]
    dv[i] = EuclideanDistance(UASPos, path(DriverPos, ptgt, true))
    DriverPos = DriverPos + DriverVelocity(tv[i]) * dt

    vsq = rowNorm(sol[1])

    ptgt = 1
    n = 10000
    c_α = 0.01
    vs = sol[1]
    ts = sol[2]
    LPos = sol[3]
    UASPos = sol[4]
    DriverPos = sol[5]
    ti = sol[6]

    OptTimeSample = sol[7][ptgt]

    μ = DriverPosition(ti, OptTimeSample, gp) + DriverPos #mean
    Σ = DriverUncertainty(ti, OptTimeSample, gp) #variance
    RDVPos = path(μ, ptgt, true)
    gpStd = sqrt(Σ + 1.0)
    gp_distr = Normal(μ, gpStd)
    x = rand(gp_distr, n)
    UASPosv = hcat(UASPos[1] * ones(n), UASPos[2] * ones(n))
    LPosv = hcat(LPos[1] * ones(n), LPos[2] * ones(n))
    EuclideanPositions = pathSample2Array(path.(x, ptgt))
    EuclideanDistancesUAS = EuclideanPositions .- UASPosv
    EuclideanDistancesL = EuclideanPositions .- LPosv

    EDS = rowNorm(EuclideanDistancesUAS')
    EDL = rowNorm(EuclideanDistancesL')

    t1 = (OptTimeSample - ti)
    #nominal
    sEDS = norm(UASPos - RDVPos)
    sEDL = norm(LPos - RDVPos)
    sVS = sEDS ./ t1
    Em, tm, vm = minReturnEnergy(RDVPos, LPos)

    #compute new velocities
    VS = EDS ./ t1
    VL = EDL ./ tm

    #compute nominal energy
    Γ1 = sVS^2 * m[1] / 2 * t1 + m[1] * alpha * t1 + Em

    #compute new energies
    E =
        VS .^ 2 .* m[1] ./ 2 .* t1 + .+VL .^ 2 .* m[2] ./ 2 .* tm .+
        m[1] * alpha * t1 .+ m[2] * alpha * tm
    xd = Γ1 .- E
    # To find our elusive distribution, we will sample from gp and fit.
    flip = -1.0

    shift = minimum(flip * xd)

    xf = flip * xd .+ flip * (shift .- 1.0)

    histogram(
        xf,
        normalized = true,
        title = "Potential Energy Loss",
        formatter = :latex,
        ylabel = L"\mathbb{P}(E\geq E^\star)",
        xlabel = L"\textrm{Energy}",
        label = "Data",
        legend = :topright
    )
    rp = range(minimum(xf),stop=maximum(xf),length=100)
    dGamma = pdf(fit(Gamma, xf), rp)
    dNormal = pdf(fit(Normal, xf), rp)
    dLogNormal = pdf(fit(LogNormal, xf), rp)
    dInverseGaussian = pdf(fit(InverseGaussian, xf), rp)

    plot!(rp, dGamma, label = "Gamma MLE Fit", lw = 3)
    plot!(rp, dNormal, label = "Normal MLE Fit", lw = 3)
    plot!(rp, dLogNormal, label = "Log Normal MLE Fit", lw = 3)
    plot!(rp, dInverseGaussian, label = "Inverse Gaussian MLE Fit", lw = 3)

    savefig("shifted_fit.pdf")

    histogram(
        xd,
        normalized = true,
        title = "Potential Energy Loss",
        formatter = :latex,
        ylabel = L"\mathbb{P}(E\geq E^\star)",
        xlabel = L"\textrm{Energy}",
        label = "Data",
        legend = :topleft
    )
    rpf = flip * rp .+ flip * (shift .- 1.0)
    plot!(rpf, dGamma, label = "Gamma MLE Fit", lw = 3)
    plot!(rpf, dNormal, label = "Normal MLE Fit", lw = 3)
    plot!(rpf, dLogNormal, label = "Log Normal MLE Fit", lw = 3)
    plot!(rpf, dInverseGaussian, label = "Inverse Gaussian MLE Fit", lw = 3)

    savefig("final_fit.pdf")

end

#TODO sunday morning:
#two plots: first like ACC, side by side with risk stuff, add above
#second, possibly: energy delta between predicted consumption and real
#we would compute this with a MPC without GP that takes in the real model
#ideally shows delta going to zero or staying negative.

function plotRiskTrajs(s, plt = true, Er = 16000)
    Random.seed!(s)
    riskv1, riskv2, iend, Evr, Evd, Eva, dv = riskcon(Er, "WorstFirst")
    riskv1 = riskv1[1:iend-3]
    riskv2 = riskv2[1:iend-3]
    Evr = Evr[1:iend-3]
    Evd = Evd[1:iend-3]
    Eva = Eva[1:iend-3]
    dv = dv[1:iend-3]

    plot(
        riskv1,
        label = L"\textrm{Main Path}",
        title = L"\textrm{CVaR Time Evolution}",
    )
    pw = plot!(
        riskv2,
        label = L"\textrm{Secondary Path}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\textrm{CVaR}_\alpha",
        formatter = :latex,
    )
    savefig("WorstFirstRisk.pdf")

    plot(
        Evr,
        label = L"E_{r}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\textrm{Energy }[J]",
        formatter = :latex,
        title = L"\textrm{Energy Allotment}",
    )
    plot!(
        Eva,
        label = L"\textrm{Abort Energy}",
        formatter = :latex,
        linestyle = :dash,
    )
    p1 = plot!(
        Evd,
        label = L"\textrm{Rendezvous Energy}",
        formatter = :latex,
        linestyle = :dash,
    )
    savefig("energies.pdf")

    p2 = plot(
        dv,
        label = L"\textrm{Distance}",
        formatter = :latex,
        ylabel = L"\textrm{Distance }[m]",
        xlabel = L"\textrm{Time }[s]",
        title = L"\textrm{UAS-Driver Distance}",
    )

    l = @layout [a b]
    plot(p1, p2, layout = l, size = (600, 400))
    savefig("ener_dist.pdf")

    if plt
        return pw
    end

    Random.seed!(s)
    riskv1b, riskv2b, iend, Evr, Evd, Eva = riskcon(Er, "BestFirst")
    riskv1b = riskv1b[1:iend-3]
    riskv2b = riskv2b[1:iend-3]

    plot(
        riskv1b,
        label = L"\textrm{Main Path}",
        title = L"\textrm{CVaR Time Evolution}",
    )
    pb = plot!(
        riskv2b,
        label = L"\textrm{Secondary Path}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\textrm{CVaR}_\alpha",
        formatter = :latex,
    )
    savefig("BestFirstRisk.pdf")

    pc1 = plot(
        riskv1,
        label = L"\textrm{Worst First}",
        title = L"\textrm{CVaR Time Evolution}",
    )
    plot!(
        riskv1b,
        label = L"\textrm{Best First}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\textrm{CVaR}_\gamma",
        formatter = :latex,
    )
    savefig("CompRisk1.pdf")

    pc2 = plot(
        riskv2,
        label = L"\textrm{Worst First}",
        title = L"\textrm{Secondary Path Risk, }\gamma=0.05",
    )
    plot!(
        riskv2b,
        label = L"\textrm{Best First}",
        xlabel = L"\textrm{Time }[s]",
        ylabel = L"\textrm{CVaR}_\gamma",
        formatter = :latex,
    )
    savefig("CompRisk2.pdf")

    #l = @layout [a b;]
    #p = plot(pc1, pc2, layout = l)

    display(pc2)

end

max_stars_repo_name: gbarsih/Multi-Path-Safe-Rendezvous

max_issues_repo_name: gbarsih/Multi-Path-Safe-Rendezvous

max_forks_repo_name: gbarsih/Multi-Path-Safe-Rendezvous


text:
using Base.Test
using SparseRF

n,m = 10^3, 5 ;
features = randn(n,m);
weights = rand(-2:2,m);
labels = features * weights;

maxdepth = 3
model = build_tree(labels, features, 5, 0, maxdepth)
@test depth(model) == maxdepth

println("\n##### nfoldCV Regression Tree #####")
r2 = nfoldCV_tree(labels, features, 3)
@test mean(r2) > 0.6

println("\n##### nfoldCV Regression Forest #####")
r2 = nfoldCV_forest(labels, features, 2, 10, 3)
@test mean(r2) > 0.8

ext: jl


text:
"""
    plot_energy(df::DataFrame)

"""
function plot_energy(df::DataFrame) end

"""
    plot_heat_capacity(df::DataFrame)

"""
function plot_heat_capacity(df::DataFrame) end

"""
    plot_magnetization(df::DataFrame)

"""
function plot_magnetization(df::DataFrame) end

"""
    plot_susceptibility(df::DataFrame)

"""
function plot_susceptibility(df::DataFrame) end

"""
    plot_2D_Ising_lattice(model::Ising)

"""
function plot_Ising_lattice(model::Ising) end


text:
#####
##### Sources
#####

eq_tends(pv::AbstractPrognosticVariable, m::LandModel, tt::Source) =
    (m.source_dt[pv]...,)

#####
##### First order fluxes
#####

eq_tends(pv::PV, land::LandModel, tt::Flux{FirstOrder}) where {PV} = (
    eq_tends(pv, land.soil.heat, tt)...,
    eq_tends(pv, land.soil.water, tt)...,
    eq_tends(pv, land.surface, tt)...,
)

eq_tends(::PV, ::AbstractSoilComponentModel, ::Flux{FirstOrder}) where {PV} = ()
eq_tends(::PV, ::NoSurfaceFlowModel, ::Flux{FirstOrder}) where {PV} = ()

eq_tends(::SurfaceWaterHeight, ::OverlandFlowModel, ::Flux{FirstOrder}) =
    (VolumeAdvection(),)

#####
##### Second order fluxes
#####

# Empty by default

eq_tends(pv::PV, ::AbstractSoilComponentModel, ::Flux{SecondOrder}) where {PV} =
    ()
eq_tends(pv::PV, ::OverlandFlowModel, ::Flux{SecondOrder}) where {PV} = ()
eq_tends(pv::PV, ::NoSurfaceFlowModel, ::Flux{SecondOrder}) where {PV} = ()

eq_tends(pv::PV, land::LandModel, tt::Flux{SecondOrder}) where {PV} = (
    eq_tends(pv, land.soil.heat, tt)...,
    eq_tends(pv, land.soil.water, tt)...,
    eq_tends(pv, land.surface, tt)...,
)

eq_tends(
    pv::PV,
    land::LandModel,
    tt::Flux{SecondOrder},
) where {PV <: VolumetricInternalEnergy} =
    (eq_tends(pv, land.soil.heat, land.soil.water, tt)...,)

eq_tends(
    ::VolumetricInternalEnergy,
    ::SoilHeatModel,
    ::SoilWaterModel,
    ::Flux{SecondOrder},
) = (DiffHeatFlux(), DarcyDrivenHeatFlux())

eq_tends(
    ::VolumetricInternalEnergy,
    ::SoilHeatModel,
    ::PrescribedWaterModel,
    ::Flux{SecondOrder},
) = (DiffHeatFlux(),)

eq_tends(::VolumetricLiquidFraction, ::SoilWaterModel, ::Flux{SecondOrder}) =
    (DarcyFlux(),)


text:
@testset "Creating" begin
	sn = SplitNode()
	ln = LeafNode([], 1)
	@test true
end

t1 = read_tree("$(dirname(pathof(TreeKnit)))/..//test/splitgraph/basic/tree1.nwk")
t2 = read_tree("$(dirname(pathof(TreeKnit)))/..//test/splitgraph/basic/tree2.nwk")
g = trees2graph(t1,t2)

@testset "Basic SplitGraph construction" begin
	@testset "Leaves" for i in 1:length(leaves(t1))
		@test g.leaves[i].conf == Int[i]
		@test g.labels_to_int[g.labels[i]] == i
	end
	#
	leaf = "C"
	idx = g.labels_to_int[leaf]
	@test g.leaves[idx].anc[1].conf == sort(Int[g.labels_to_int[x] for x in ["C","D"]])
	@test g.leaves[idx].anc[1].anc.conf == sort(Int[g.labels_to_int[x] for x in ["C","D","E"]])
	@test g.leaves[idx].anc[1].anc.anc.conf == sort(Int[g.labels_to_int[x] for x in ["C","D","E","A","B"]])
	@test g.leaves[idx].anc[2].conf == sort(Int[g.labels_to_int[x] for x in ["C","B"]])
	@test g.leaves[idx].anc[2].anc.conf == sort(Int[g.labels_to_int[x] for x in ["C","B","A"]])
	@test g.leaves[idx].anc[2].anc.anc.conf == sort(Int[g.labels_to_int[x] for x in ["C","D","E","A","B"]])
end

@testset "Basic compute_energy" begin
	conf = ones(Bool, length(g.leaves))
	@test compute_energy(conf, g) == 5
	@testset for i in Iterators.filter(!=(3), 1:5)
		conf[i] = false
		@test compute_energy(conf, g) == 4
		conf[i] = true
	end
	conf = Bool[1, 1, 0, 1, 1]
	@test compute_energy(conf, g) == 0
	@testset for i in 1:5
		conf[i] = false
		@test compute_energy(conf, g) == 0
	end
end

# For exactly matching trees
t1 = node2tree(parse_newick("((A,B),(C,D))"))
t2 = node2tree(parse_newick("((A,B),(C,D))"))
g = trees2graph(t1,t2);

@testset "Compute energy for identical trees" begin
	conf = ones(Bool, length(g.leaves))
	@test compute_energy(conf, g) == 0
	@test SplitGraph.count_mismatches(t1,t2) == 0
end

max_stars_repo_name: PierreBarrat/RecombTools

max_issues_repo_name: PierreBarrat/TreeKnit

max_forks_repo_name: PierreBarrat/TreeKnit


text:
using NodeJS
using Test

@testset "NodeJS" begin

    node_v = chomp(read(`$(nodejs_cmd()) -v`, String))
    npm_v = chomp(read(`$(npm_cmd()) -v`, String))

    if NodeJS.nodejs_path !== nothing
        @test node_v == "v12.22.1"
        @test npm_v == "6.14.12"
    end

end


text:
using SPADL

@time test = get_events_data(:wyscout, download = false, path = "wyscout_data")

GC.gc()

check = competitions(test)

match_test = games(test, 524, 181248)

teams_test = teams(test, 2576335)

players_test = players(test, 2576335)

# we have to update the loader first
@time test = get_events(test, "events_Italy.json");

@time events_df = events(test, 2576336);

events_df[1]


text:
module PackageHelper

using JSON

export pkgpath, depotpath
export pkghash, pkghash_tree, pkghash_short
export latesthash, latesthash_tree

function print_error()
    println(Base.stderr, "Update check failed")
    for (exc, bt) in Base.catch_stack()
        showerror(Base.stderr, exc, bt)
        println(Base.stderr, "")
    end
end

pkgpath(pkg) = pathof(pkg) |> dirname |> dirname
depotpath(pkg) = pkgpath(pkg) |> dirname |> dirname |> dirname

function pkghash_tree()
    try
        ctx = Pkg.Types.Context()
        return string(ctx.env.manifest[ctx.env.project.deps["Ahorn"]].tree_hash)
    catch e
        return nothing
    end
end

function pkghash()
    try
        clonespath = joinpath(depotpath(Ahorn), "clones")
        for clone in readdir(clonespath)
            clonepath = joinpath(clonespath, clone)
            configpath= joinpath(clonepath, "config")
            if isfile(configpath)
                config = read(configpath, String)
                if !occursin("url = https://github.com/CelestialCartographers/Ahorn.git", config)
                    continue
                end
                headpath = joinpath(clonepath, "FETCH_HEAD")
                head = read(headpath, String)
                hash = match(r"([0-9a-f]+)[^\n]+'master'", head).captures[1]
                return hash
            end
        end
    catch e
        return nothing
    end

    return nothing
end

function pkghash_short()
    h = pkghash()
    if h !== nothing
       h = "$(h[1:7]) (git)"
    else
        h = pkghash_tree()
        if h !== nothing
            h = "$(h[1:7]) (pkg)"
        end
    end
    return h
end

const apiurl = "https://api.github.com/repos/CelestialCartographers/Ahorn/commits/master"

function latesthash()
    try
        path = download(apiurl)
        json = open(JSON.parse, path)
        return json["sha"]
    catch e
        print_error()
        return nothing
    end
end

function latesthash_tree()
    try
        path = download(apiurl)
        json = open(JSON.parse, path)
        return json["commit"]["tree"]["sha"]
    catch e
        print_error()
        return nothing
    end
end

end

max_stars_repo_name: CelestialCartographers/Ahorn

max_issues_repo_name: CelestialCartographers/Ahorn

max_forks_repo_name: CelestialCartographers/Ahorn


text:
@doc doc"""

    PowerLens(ϕ, order)

`PowerLens` is a lensing operator which lenses a map with a power
series expansion in $\nabla \phi$ to any order.

```math
f(x+\nabla x) \approx f(x) + (\nabla f)(\nabla \phi) + \frac{1}{2} (\nabla \nabla f) (\nabla \phi)^2 + ...
```

The action of the operator and its adjoint can be computed.
"""
struct PowerLens{D<:Dict} <: ImplicitOp{Bottom}
    order :: Int
    ∇1ϕᵖ  :: D
    ∇2ϕᵖ  :: D
end

PowerLens(order) = x -> PowerLens(x, order)
PowerLens(ϕ::Field, order) = PowerLens(∇*ϕ, order)
function PowerLens(d::FieldVector, order)
    require_unbatched(d[1])
    ∇1ϕ,  ∇2ϕ  = Ł(d)
    ∇1ϕᵖ, ∇2ϕᵖ = (Dict(p => (p==0 ? 1 : ∇iϕ.^p) for p=0:order) for ∇iϕ=(∇1ϕ,∇2ϕ))
    PowerLens(order, ∇1ϕᵖ, ∇2ϕᵖ)
end

"""
    antilensing(L::PowerLens)

Create a `PowerLens` operator that lenses by `-ϕ` instead.
"""
function antilensing(L::PowerLens)
    PowerLens(N, (Dict(p=>∇iϕᵖ*(-1)^p for (p,∇iϕᵖ)=coeffs) for coeffs=(L.∇1ϕᵖ,L.∇1ϕᵖ))...)
end

function *(L::PowerLens, f::Field)
    require_unbatched(f)
    Ðf = Ð(f)
    f̃ = copy(Ł(f))
    for n in 1:L.order, (a,b) in zip(0:n,n:-1:0)
        @. f̃ += L.∇1ϕᵖ[a] * L.∇2ϕᵖ[b] * $(Ł(∇[1]^a * ∇[2]^b * Ðf)) / factorial(a) / factorial(b)
    end
    f̃
end

function *(Ladj::Adjoint{<:Any,<:PowerLens}, f::Field)
    require_unbatched(f)
    L = parent(Ladj)
    Łf = Ł(f)
    r = copy(Ð(f))
    for n in 1:L.order, (a,b) in zip(0:n,n:-1:0)
        r .+= (-1)^n .* (∇[1]^a * ∇[2]^b * Ð(@. L.∇1ϕᵖ[a] * L.∇2ϕᵖ[b] * Łf)) ./ factorial(a) ./ factorial(b)
    end
    r
end


text:
#=

  Just forgotten puzzle (Enigma 1517) in Julia ConstraintSolver.jl

  From http://www.f1compiler.com/samples/Enigma 201517.f1.html
  """
  Enigma 1517 Bob Walker, New Scientist magazine, October 25, 2008.

  Joe was furious when he forgot one of his bank account numbers.
  He remembered that it had all the digits 0 to 9 in some order, so he tried
  the following four sets without success:

      9 4 6 2 1 5 7 8 3 0
      8 6 0 4 3 9 1 2 5 7
      1 6 4 0 2 9 7 8 5 3
      6 8 2 4 3 1 9 0 7 5

  When Joe finally remembered his account number, he realised that in each set
  just four of the digits were in their correct position and that, if one knew
  that, it was possible to work out his account number.
  What was it?
  """

  Model created by Hakan Kjellerstrand, hakank@gmail.com
  See also my Julia page: http://www.hakank.org/julia/

=#
using ConstraintSolver, JuMP
using Cbc, GLPK, Ipopt
const CS = ConstraintSolver
include("constraints_utils.jl")

function just_forgotten(print_solutions=true,all_solutions=true)

    cbc_optimizer = optimizer_with_attributes(Cbc.Optimizer, "logLevel" => 0)
    glpk_optimizer = optimizer_with_attributes(GLPK.Optimizer)
    ipopt_optimizer = optimizer_with_attributes(Ipopt.Optimizer)

    model = Model(optimizer_with_attributes(CS.Optimizer,   "all_solutions"=> all_solutions,
                                                            # "all_optimal_solutions"=>true,
                                                            "logging"=>[],

                                                            "traverse_strategy"=>:BFS,
                                                            # "traverse_strategy"=>:DFS,
                                                            # "traverse_strategy"=>:DBFS,

                                                            # "branch_split"=>:Smallest,
                                                            "branch_split"=>:Biggest,
                                                            # "branch_split"=>:InHalf,

                                                            # https://wikunia.github.io/ConstraintSolver.jl/stable/options/#branch_strategy-(:Auto)
                                                            "branch_strategy" => :IMPS, # default
                                                            # "branch_strategy" => :ABS, # Activity Based Search
                                                            # "activity.decay" => 0.999, # default 0.999
                                                            # "activity.max_probes" => 100, # default, 10
                                                            # "activity.max_confidence_deviation" => 20, # default 20

                                                            # "simplify"=>false,
                                                            # "simplify"=>true, # default

                                                            "time_limit"=>6,

                                                            # "backtrack" => false, # default true
                                                            # "backtrack_sorting" => false, # default true

                                                            # "lp_optimizer" => cbc_optimizer,
                                                            # "lp_optimizer" => glpk_optimizer,
                                                            # "lp_optimizer" => ipopt_optimizer,
                                        ))
    a = resize_matrix([[9,4,6,2,1,5,7,8,3,0],
                       [8,6,0,4,3,9,1,2,5,7],
                       [1,6,4,0,2,9,7,8,5,3],
                       [6,8,2,4,3,1,9,0,7,5]])
    rows,cols = size(a)
    @variable(model, 0 <= x[1:cols] <= 9, Int)
    @constraint(model, x in CS.AllDifferentSet())

    for row in eachrow(a)
        # These two versions don't work in ConstraintSolver.jl v0.6.0
        # @constraint(model, sum(x .== row) == 4)
        # @constraint(model, sum([x[i] == row[i] for i in 1:cols]) == 4)

        # booleans to the rescue!
        b = @variable(model, [1:cols], Bin)
        for i in 1:cols
            @constraint(model,b[i] := {x[i] == row[i]})
        end
        @constraint(model, sum(b) == 4)
    end

    # Solve the problem
    optimize!(model)

    status = JuMP.termination_status(model)
    # println("status:$status")
    if status == MOI.OPTIMAL
        num_sols = MOI.get(model, MOI.ResultCount())
        println("num_sols:$num_sols\n")
        if print_solutions
            for sol in 1:num_sols
                println("solution #$sol")
                x_val = convert.(Integer,JuMP.value.(x; result=sol))
                println("x:$x_val")

            end
        end
    else
        println("status:$status")
    end

    return status
end

println("Proving unicity")
@time just_forgotten(true,true)
println("\n'First' solution")
@time just_forgotten(true,false)

max_stars_repo_name: Wikunia/hakank

max_issues_repo_name: Wikunia/hakank

max_forks_repo_name: Wikunia/hakank


text:
using CCDReduction: getdata

@testset "bias subtraction(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    hdu_bias_frame = CCDData(M6707HH[1])
    array_frame = getdata(M6707HH[1])
    array_bias_frame = getdata(M6707HH[1])
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")
    string_bias_frame = joinpath(@__DIR__, "data/M6707HH.fits")

    # non-mutating version
    # testing CCDData CCDData case
    processed_frame = subtract_bias(hdu_frame, hdu_bias_frame)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, hdu_frame)

    # testing Array CCDData case
    processed_frame = subtract_bias(array_frame, hdu_bias_frame)
    @test processed_frame isa Array
    @test processed_frame == zeros(1059, 1059)

    # testing CCDData Array case
    processed_frame = subtract_bias(hdu_frame, array_bias_frame)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, hdu_frame)

    # testing CCDData String case
    processed_frame = subtract_bias(hdu_frame, string_bias_frame; hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, hdu_frame)

    # testing String CCDData case
    processed_frame = subtract_bias(string_frame, hdu_bias_frame; hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, hdu_frame)

    # testing String String case
    processed_frame = subtract_bias(string_frame, string_bias_frame; hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, hdu_frame)

    # testing Array String case
    processed_frame = subtract_bias(array_frame, string_bias_frame)
    @test processed_frame isa Array
    @test processed_frame == zeros(1059, 1059)

    # testing String Array case
    processed_frame = subtract_bias(string_frame, array_bias_frame)
    @test processed_frame isa CCDData
    @test processed_frame == zeros(1059, 1059)

    # testing mutating version
    # testing CCDData CCDData case
    hdu_frame = CCDData(M6707HH[1])
    subtract_bias!(hdu_frame, hdu_bias_frame)
    @test hdu_frame isa CCDData
    @test hdu_frame.data == zeros(1059, 1059)

    # testing CCDData Array case
    hdu_frame = CCDData(M6707HH[1])
    subtract_bias!(hdu_frame, array_bias_frame)
    @test hdu_frame isa CCDData
    @test hdu_frame.data == zeros(1059, 1059)

    # testing Array CCDData case
    array_frame = getdata(M6707HH[1])
    subtract_bias!(array_frame, hdu_bias_frame)
    @test array_frame isa Array
    @test array_frame == zeros(1059, 1059)

    # testing Array String
    array_frame = getdata(M6707HH[1])
    subtract_bias!(array_frame, string_bias_frame)
    @test array_frame isa Array
    @test array_frame == zeros(1059, 1059)

    # testing CCDData String
    hdu_frame = CCDData(M6707HH[1])
    subtract_bias!(hdu_frame, string_bias_frame)
    @test hdu_frame isa CCDData
    @test hdu_frame.data == zeros(1059, 1059)

    # testing error in mutating version
    hdu_frame = CCDData(fill(2, 5, 5))
    hdu_bias = CCDData(fill(2.5, 5, 5))
    @test_throws InexactError subtract_bias!(hdu_frame, hdu_bias)
end

@testset "overscan subtraction(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    array_frame = getdata(M6707HH[1])
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")

    # testing non-mutating version
    # testing CCDData case
    processed_frame = subtract_overscan(hdu_frame, (:, 1050:1059))
    @test processed_frame isa CCDData
    @test processed_frame.data == subtract_overscan(array_frame, (:, 1050:1059))
    test_header(processed_frame, hdu_frame)

    processed_frame = subtract_overscan(hdu_frame, "1050:1059, 1:1059")
    processed_frame isa CCDData
    @test processed_frame.data == subtract_overscan(array_frame, (:, 1050:1059))

    # testing String case
    processed_frame = subtract_overscan(string_frame, "1050:1059, 1:1059"; hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == subtract_overscan(array_frame, (:, 1050:1059))
    test_header(processed_frame, hdu_frame)

    # testing mutating version
    hdu_frame = CCDData(M6707HH[1])
    subtract_overscan!(hdu_frame, (:, 1050:1059))
    @test hdu_frame.data == subtract_overscan(array_frame, (:, 1050:1059))

    hdu_frame = CCDData(M6707HH[1])
    subtract_overscan!(hdu_frame, "1050:1059, 1:1059")
    @test hdu_frame.data == subtract_overscan(array_frame, (:, 1050:1059))
end

@testset "flat correction(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    hdu_flat_frame = CCDData(M6707HH[1])
    array_frame = getdata(M6707HH[1])
    array_flat_frame = getdata(M6707HH[1])
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")
    string_flat_frame = joinpath(@__DIR__, "data/M6707HH.fits")
    mean_flat_frame = mean(array_flat_frame)

    # testing non mutating version
    # testing CCDData CCDData case
    processed_frame = flat_correct(hdu_frame, hdu_flat_frame)
    @test processed_frame isa CCDData
    test_header(processed_frame, hdu_frame)
    @test processed_frame.data ≈ fill(mean_flat_frame, 1059, 1059)

    # testing CCDData Array case
    processed_frame = flat_correct(hdu_frame, array_flat_frame; norm_value = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ ones(1059, 1059)

    # testing Array CCDData case
    processed_frame = flat_correct(array_frame, hdu_flat_frame; norm_value = 1)
    @test processed_frame isa Array
    @test processed_frame ≈ ones(1059, 1059)

    # testing String CCDData case
    processed_frame = flat_correct(string_frame, hdu_flat_frame; norm_value = 1, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ ones(1059, 1059)

    # testing String Array Case
    processed_frame = flat_correct(string_frame, array_flat_frame; norm_value = 1, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ ones(1059, 1059)
    test_header(processed_frame, CCDData(string_frame; hdu = 1))

    # testing CCDData String case
    processed_frame = flat_correct(hdu_frame, string_flat_frame; norm_value = 1, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ ones(1059, 1059)

    # testing Array String case
    processed_frame = flat_correct(array_frame, string_flat_frame; norm_value = 1, hdu = 1)
    @test processed_frame isa Array
    @test processed_frame ≈ ones(1059, 1059)

    # testing String String case
    processed_frame = flat_correct(string_frame, string_flat_frame; norm_value = 1, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ ones(1059, 1059)
    test_header(processed_frame, CCDData(string_frame; hdu = 1))

    # testing type mutation in non mutating version
    hdu_frame = CCDData(fill(1, 5, 5))
    bias_frame = CCDData(fill(2.0, 5, 5))
    processed_frame = flat_correct(hdu_frame, bias_frame; norm_value = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data ≈ fill(0.5, 5, 5)

    # testing mutating version
    # testing CCDData Array case
    hdu_frame = CCDData(ones(5, 5))
    flat_correct!(hdu_frame, fill(2.0, 5, 5); norm_value = 1)
    @test hdu_frame.data ≈ fill(0.5, 5, 5)

    # testing CCDData CCDData case
    hdu_frame = CCDData(ones(5, 5))
    flat_correct!(hdu_frame, CCDData(fill(2.0, 5, 5)); norm_value = 1)
    @test hdu_frame.data ≈ fill(0.5, 5, 5)

    # testing CCDData String case
    hdu_frame = CCDData(M6707HH[1])
    flat_correct!(hdu_frame, string_flat_frame; norm_value = 1, hdu = 1)
    @test hdu_frame.data ≈ ones(1059, 1059)

    # testing Array String case
    array_frame = getdata(M6707HH[1])
    flat_correct!(array_frame, string_flat_frame; norm_value = 1, hdu = 1)
    @test hdu_frame.data ≈ ones(1059, 1059)

    # testing type error in mutating version
    frame = CCDData(fill(1, 5 ,5))
    @test_throws InexactError flat_correct!(frame, fill(2.0, 5, 5); norm_value = 1)
end

@testset "trim(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    array_frame = M6707HH[1] |> getdata
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")

    # testing trim
    # testing CCDData case
    processed_frame = trim(hdu_frame, (:, 1050:1059))
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test processed_frame.data == trim(array_frame, (:, 1050:1059))

    processed_frame = trim(hdu_frame, "1:1059, 1050:1059")
    test_header(processed_frame, hdu_frame)
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test processed_frame.data == trim(array_frame, (1050:1059, :))

    # testing string case
    processed_frame = trim(string_frame, "1:1059, 1050:1059"; hdu = 1)
    test_header(processed_frame, hdu_frame)
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test processed_frame.data == trim(array_frame, (1050:1059, :))

    # testing trimview
    processed_frame = trimview(hdu_frame, (:, 1050:1059))
    @test processed_frame isa CCDData
    @test processed_frame.data isa SubArray
    processed_frame.data[3] = 16
    @test hdu_frame.data[3] == 16 # modifying processed_frame modifies hdu_frame
end

@testset "cropping(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    array_frame = getdata(M6707HH[1])
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")

    # testing crop
    # testning CCDData case
    processed_frame = crop(hdu_frame, (:, 5))
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test processed_frame.data == crop(array_frame, (:, 5))

    @test_logs (:warn, "dimension 1 changed from 348 to 349") (:warn, "dimension 2 changed from 226 to 227") processed_frame = crop(hdu_frame, (348, 226))
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test_logs (:warn, "dimension 1 changed from 348 to 349") (:warn, "dimension 2 changed from 226 to 227") @test processed_frame.data == crop(array_frame, (348, 226))

    processed_frame = crop(hdu_frame, (1000, 5); force_equal = false)
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    @test processed_frame.data == crop(array_frame, (1000, 5); force_equal = false)

    # testing String case
    processed_frame = crop(string_frame, (1000, 5); force_equal = false, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data isa Array
    test_header(processed_frame, CCDData(string_frame; hdu = 1))
    @test processed_frame.data == crop(array_frame, (1000, 5); force_equal = false)

    # testing trimview
    processed_frame = cropview(hdu_frame, (:, :))
    @test processed_frame isa CCDData
    @test processed_frame.data isa SubArray
    processed_frame.data[5] = 1
    @test hdu_frame[5] == 1 # modifying processed_frame modifies hdu_frame
end

@testset "combine(FITS)" begin
    # setting initial data
    frame = CCDData(M6707HH[1])
    vector_frames = [frame for i in 1:3]
    vector_arrays = [frame.data for i in 1:3]
    dir_frame = joinpath(@__DIR__, "data/M6707HH.fits")
    array_frame = getdata(M6707HH[1])

    # testing the vector version
    processed_frame = combine(vector_frames)
    @test processed_frame isa CCDData
    @test processed_frame.data == combine(vector_arrays)

    # testing varargs version
    processed_frame = combine(frame, frame, frame)
    @test processed_frame isa CCDData
    @test processed_frame.data == combine(vector_arrays)

    # testing header header copyig functionality
    data1 = CCDData(ones(5, 6))
    data2 = CCDData(fill(2, 5, 6))
    data2.hdr["SIMPLE"] = false # initially was true

    processed_frame = combine(data1, data2; header_hdu = 2, method = sum)
    @test processed_frame isa CCDData
    @test processed_frame.data == combine(data1.data, data2.data; method = sum)
    test_header(processed_frame, data2)

    # testing String case
    processed_frame = combine([dir_frame, dir_frame, dir_frame]; method = sum)
    @test processed_frame isa CCDData
    @test processed_frame.data == combine(array_frame, array_frame, array_frame; method = sum)
    test_header(processed_frame, frame)
end

@testset "dark subtraction(FITS)" begin
    # setting initial data
    hdu_frame = CCDData(M6707HH[1])
    hdu_dark_frame = CCDData(M6707HH[1])
    array_frame = getdata(M6707HH[1])
    array_dark_frame = getdata(M6707HH[1])
    string_frame = joinpath(@__DIR__, "data/M6707HH.fits")
    string_dark_frame = joinpath(@__DIR__, "data/M6707HH.fits")

    # testing non-mutating version
    # testing CCDData CCDData case
    processed_frame = subtract_dark(hdu_frame, hdu_dark_frame; dark_exposure = 0.5)
    @test processed_frame isa CCDData
    test_header(processed_frame, hdu_frame)
    @test processed_frame == (-1) .* array_frame

    # testing CCDData Array case
    processed_frame = subtract_dark(hdu_frame, array_dark_frame; dark_exposure = 2, data_exposure = 2)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)

    # testing Array CCDData case
    processed_frame = subtract_dark(array_frame, hdu_dark_frame; dark_exposure = 2, data_exposure = 2)
    @test processed_frame isa Array
    @test processed_frame == zeros(1059, 1059)

    # testing CCDData String case
    processed_frame = subtract_dark(hdu_frame, string_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)

    # testing Array String case
    processed_frame = subtract_dark(array_frame, string_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test processed_frame isa Array
    @test processed_frame == zeros(1059, 1059)

    # testing String Array case
    processed_frame = subtract_dark(string_frame, array_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)

    # testing String CCDData case
    processed_frame = subtract_dark(string_frame, hdu_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)
    test_header(processed_frame, CCDData(string_frame; hdu = 1))

    # testing String String case
    processed_frame = subtract_dark(string_frame, string_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test processed_frame isa CCDData
    @test processed_frame.data == zeros(1059, 1059)

    # testing mutating version
    # testing CCDData CCDData case
    hdu_frame = CCDData(M6707HH[1])
    subtract_dark!(hdu_frame, hdu_dark_frame; dark_exposure = 2, data_exposure = 2)
    @test hdu_frame.data == zeros(1059, 1059)

    # testing Array CCDData case
    array_frame = getdata(M6707HH[1])
    subtract_dark!(array_frame, hdu_dark_frame; dark_exposure = 2, data_exposure = 2)
    @test hdu_frame.data == zeros(1059, 1059)

    # testing CCDData String case
    hdu_frame = CCDData(M6707HH[1])
    subtract_dark!(hdu_frame, string_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test hdu_frame.data == zeros(1059, 1059)

    # testing Array String case
    array_frame = getdata(M6707HH[1])
    subtract_dark!(array_frame, string_dark_frame; dark_exposure = 2, data_exposure = 2, hdu = 1)
    @test hdu_frame.data == zeros(1059, 1059)
end

@testset "helper(FITS)" begin
    # testing getdata
    hdu = M6707HH[1]
    data = read(hdu)'
    @test data == getdata(hdu)

    # testing the ways to access header
    ccd = CCDData(zeros(5, 5))
    @test ccd[:SIMPLE] == true
    @test ccd[:SIMPLE] == ccd["SIMPLE"]
    ccd[:SIMPLE] = false
    @test ccd["SIMPLE"] == false # testing the modified version
end


text:
using BinaryProvider # requires BinaryProvider 0.3.0 or later

# Parse some basic command-line arguments
const verbose = "--verbose" in ARGS
const prefix = Prefix(get([a for a in ARGS if a != "--verbose"], 1, joinpath(@__DIR__, "usr")))
products = [
    LibraryProduct(prefix, ["libgettext"], :libgettext),
]

# Download binaries from hosted location
bin_prefix = "https://github.com/JuliaBinaryWrappers/Gettext_jll.jl/releases/download/Gettext-v0.20.1+0"

# Listing of files generated by BinaryBuilder:
download_info = Dict(
    Linux(:aarch64, libc=:glibc) => ("$bin_prefix/Gettext.v0.20.1.aarch64-linux-gnu.tar.gz", "088b2a3a3ed4f9308a4075b103325d21dbe979cc5f4b7b519e24ca653383b9fc"),
    Linux(:aarch64, libc=:musl) => ("$bin_prefix/Gettext.v0.20.1.aarch64-linux-musl.tar.gz", "139a57ad6208dd45fb3fb6dcc5127adc08a614ad6229b7e750f545b75e5a8e9a"),
    Linux(:armv7l, libc=:glibc, call_abi=:eabihf) => ("$bin_prefix/Gettext.v0.20.1.arm-linux-gnueabihf.tar.gz", "3ae5dc143318c6b5522a42b064f697cd846dfc0ed85841b6d06849cf52417f27"),
    Linux(:armv7l, libc=:musl, call_abi=:eabihf) => ("$bin_prefix/Gettext.v0.20.1.arm-linux-musleabihf.tar.gz", "4aee2e1833728922573f90251e70c3f18bf7c5c4781f54e228c569b71ab41a73"),
    Linux(:i686, libc=:glibc) => ("$bin_prefix/Gettext.v0.20.1.i686-linux-gnu.tar.gz", "694ea74b382c91c06660ccc4ebc7e0cce28aac46c9f596d2851a810f6fa7e9b4"),
    Linux(:i686, libc=:musl) => ("$bin_prefix/Gettext.v0.20.1.i686-linux-musl.tar.gz", "fd7ad287c278855fa02abb6be15b423525266f43bcf53d6802f2af6b40313ea3"),
    Windows(:i686) => ("$bin_prefix/Gettext.v0.20.1.i686-w64-mingw32.tar.gz", "f667caf54f21fa9e6724ac1b96528dc754d95fa4406eb8ded8d59335015d5248"),
    Linux(:powerpc64le, libc=:glibc) => ("$bin_prefix/Gettext.v0.20.1.powerpc64le-linux-gnu.tar.gz", "ad1a6e81cde9d0da01084cd9c014a665461229cecb6d05fe60657f45c4786a03"),
    MacOS(:x86_64) => ("$bin_prefix/Gettext.v0.20.1.x86_64-apple-darwin14.tar.gz", "058ff69545db8ea66a70afb9d9991a824b471a88de48daa0a416daa131c68c80"),
    Linux(:x86_64, libc=:glibc) => ("$bin_prefix/Gettext.v0.20.1.x86_64-linux-gnu.tar.gz", "e6f9b82be1cee60184eb919fadba49f16044539d5902f612343d921c086aeab9"),
    Linux(:x86_64, libc=:musl) => ("$bin_prefix/Gettext.v0.20.1.x86_64-linux-musl.tar.gz", "0c32bf677cc0df897b26ab5b5e3a1e23a812e1bc0cddaa8f5abe314c7c5bf0c2"),
    FreeBSD(:x86_64) => ("$bin_prefix/Gettext.v0.20.1.x86_64-unknown-freebsd11.1.tar.gz", "391f30c66ef34f26d64fe74954cd9c7c184b6762409a29fcb9ff54421b5e8f25"),
    Windows(:x86_64) => ("$bin_prefix/Gettext.v0.20.1.x86_64-w64-mingw32.tar.gz", "222f6e71ef3cfe3f3bdb089984e422e93cb65d138a835c764e6fd88b81c03ed6"),
)

# Install unsatisfied or updated dependencies:
unsatisfied = any(!satisfied(p; verbose=verbose) for p in products)
dl_info = choose_download(download_info, platform_key_abi())
if dl_info === nothing && unsatisfied
    # If we don't have a compatible .tar.gz to download, complain.
    # Alternatively, you could attempt to install from a separate provider,
    # build from source or something even more ambitious here.
    error("Your platform (\"$(Sys.MACHINE)\", parsed as \"$(triplet(platform_key_abi()))\") is not supported by this package!")
end

# If we have a download, and we are unsatisfied (or the version we're
# trying to install is not itself installed) then load it up!
if unsatisfied || !isinstalled(dl_info...; prefix=prefix)
    # Download and install binaries
    install(dl_info...; prefix=prefix, force=true, verbose=verbose)
end

# Write out a deps.jl file that will contain mappings for our products
write_deps_file(joinpath(@__DIR__, "deps.jl"), products, verbose=verbose)


text:
export ScaledInterpolation, eachvalue

import Base: iterate

struct ScaledInterpolation{T,N,ITPT,IT,RT} <: AbstractInterpolationWrapper{T,N,ITPT,IT}
    itp::ITPT
    ranges::RT
end

Base.parent(A::ScaledInterpolation) = A.itp
count_interp_dims(::Type{<:ScaledInterpolation{T,N,ITPT}}, n) where {T,N,ITPT} = count_interp_dims(ITPT, n)
BoundsCheckStyle(sitp::ScaledInterpolation) = BoundsCheckStyle(sitp.itp)

"""
`scale(itp, xs, ys, ...)` scales an existing interpolation object to allow for indexing using other coordinate axes than unit ranges, by wrapping the interpolation object and transforming the indices from the provided axes onto unit ranges upon indexing.

The parameters `xs` etc must be either ranges or linspaces, and there must be one coordinate range/linspace for each dimension of the interpolation object.

For every `NoInterp` dimension of the interpolation object, the range must be exactly `1:size(itp, d)`.
"""
function scale(itp::AbstractInterpolation{T,N,IT}, ranges::Vararg{AbstractRange,N}) where {T,N,IT}
    check_ranges(itpflag(itp), axes(itp), ranges)
    ScaledInterpolation{T,N,typeof(itp),IT,typeof(ranges)}(itp, ranges)
end

function check_ranges(flags, axs, ranges)
    check_range(getfirst(flags), axs[1], ranges[1])
    check_ranges(getrest(flags), Base.tail(axs), Base.tail(ranges))
end
check_ranges(::Any, ::Tuple{}, ::Tuple{}) = nothing

check_range(::NoInterp, ax, r) = ax == r || throw(ArgumentError("The range $r did not equal the corresponding axis of the interpolation object $ax"))
check_range(::Any, ax, r) = length(ax) == length(r) || throw(ArgumentError("The range $r is incommensurate with the corresponding axis $ax"))

# With regards to size and [], ScaledInterpolation behaves like the underlying interpolation object
size(sitp::ScaledInterpolation) = size(sitp.itp)
axes(sitp::ScaledInterpolation) = axes(sitp.itp)

itpflag(sitp::ScaledInterpolation) = itpflag(sitp.itp)

@propagate_inbounds function Base.getindex(sitp::ScaledInterpolation{T,N}, i::Vararg{Int,N}) where {T,N}
    sitp.itp[i...]
end

lbounds(sitp::ScaledInterpolation) = _lbounds(sitp.ranges, itpflag(sitp.itp))
ubounds(sitp::ScaledInterpolation) = _ubounds(sitp.ranges, itpflag(sitp.itp))

boundstep(r::StepRange) = r.step / 2
boundstep(r::UnitRange) = 1//2
"""
Returns *half* the width of one step of the range.

This function is used to calculate the upper and lower bounds of `OnCell` interpolation objects.
""" boundstep

lbound(ax::AbstractRange, ::DegreeBC, ::OnCell) = first(ax) - boundstep(ax)
ubound(ax::AbstractRange, ::DegreeBC, ::OnCell) = last(ax) + boundstep(ax)
lbound(ax::AbstractRange, ::DegreeBC, ::OnGrid) = first(ax)
ubound(ax::AbstractRange, ::DegreeBC, ::OnGrid) = last(ax)

# For (), we scale the evaluation point
@propagate_inbounds function (sitp::ScaledInterpolation{T,N})(xs::Vararg{Number,N}) where {T,N}
    @boundscheck (checkbounds(Bool, sitp, xs...) || Base.throw_boundserror(sitp, xs))
    xl = maybe_clamp(sitp.itp, coordslookup(itpflag(sitp.itp), sitp.ranges, xs))
    @inbounds sitp.itp(xl...)
end
@inline function (sitp::ScaledInterpolation)(x::Vararg{UnexpandedIndexTypes})
    xis = to_indices(sitp, x)
    xis == x && error("evaluation not supported for ScaledInterpolation at positions $x")
    sitp(xis...)
end

(sitp::ScaledInterpolation{T,1}, x::Number, y::Int) where {T} = y == 1 ? sitp(x) : Base.throw_boundserror(sitp, (x, y))

@inline function (itp::ScaledInterpolation{T,N})(x::Vararg{Union{Number,AbstractVector},N}) where {T,N}
    [itp(i...) for i in Iterators.product(x...)]
end

@inline function coordslookup(flags, ranges, xs)
    item = coordlookup(getfirst(flags), ranges[1], xs[1])
    (item, coordslookup(getrest(flags), Base.tail(ranges), Base.tail(xs))...)
end
coordslookup(::Any, ::Tuple{}, ::Tuple{}) = ()

coordlookup(::NoInterp, r, i) = i
coordlookup(::Flag, r, x) = coordlookup(r, x)

coordlookup(r::UnitRange, x) = x - r.start + oneunit(eltype(r))
# coordlookup(i::Bool, r::AbstractRange, x) = i ? coordlookup(r, x) : convert(typeof(coordlookup(r,x)), x)
coordlookup(r::StepRange, x) = (x - r.start) / r.step + oneunit(eltype(r))

coordlookup(r::StepRangeLen, x) = (x - first(r)) / step(r) + oneunit(eltype(r))
boundstep(r::StepRangeLen) = 0.5*step(r)
rescale_gradient(r::StepRangeLen, g) = g / step(r)

basetype(::Type{ScaledInterpolation{T,N,ITPT,IT,RT}}) where {T,N,ITPT,IT,RT} = ITPT
basetype(sitp::ScaledInterpolation) = basetype(typeof(sitp))

@propagate_inbounds function gradient(sitp::ScaledInterpolation{T,N}, xs::Vararg{Number,N}) where {T,N}
    @boundscheck (checkbounds(Bool, sitp, xs...) || Base.throw_boundserror(sitp, xs))
    xl = maybe_clamp(sitp.itp, coordslookup(itpflag(sitp.itp), sitp.ranges, xs))
    g = gradient(sitp.itp, xl...)
    SVector(rescale_gradient_components(itpflag(sitp.itp), sitp.ranges, Tuple(g)))
end

function rescale_gradient_components(flags, ranges, g)
    if getfirst(flags) isa NoInterp
        return rescale_gradient_components(getrest(flags), Base.tail(ranges), g)    # don't consume a coordinate of g
    else
        item = rescale_gradient(ranges[1], g[1])
        return (item, rescale_gradient_components(getrest(flags), Base.tail(ranges), Base.tail(g))...)
    end
end
rescale_gradient_components(flags, ::Tuple{}, ::Tuple{}) = ()

rescale_gradient(r::StepRange, g) = g / r.step
rescale_gradient(r::UnitRange, g) = g

"""
`rescale_gradient(r::AbstractRange)`

Implements the chain rule dy/dx = dy/du * du/dx for use when calculating gradients with scaled interpolation objects.
""" rescale_gradient

### Iteration

struct ScaledIterator{SITPT,CI,WIS}
    sitp::SITPT   # ScaledInterpolation object
    ci::CI        # the CartesianIndices object
    wis::WIS      # WeightedIndex vectors
    breaks1::Vector{Int}   # breaks along dimension 1 where new evaluations must occur
end

Base.IteratorSize(::Type{ScaledIterator{SITPT,CI,WIS}}) where {SITPT,CI<:CartesianIndices{N},WIS} where N = Base.HasShape{N}()
Base.axes(iter::ScaledIterator) = axes(iter.ci)
Base.size(iter::ScaledIterator) = size(iter.ci)

struct ScaledIterState{N,V}
    cistate::CartesianIndex{N}
    ibreak::Int
    cached_evaluations::NTuple{N,V}
end

function eachvalue(sitp::ScaledInterpolation{T,N}) where {T,N}
    itps = tcollect(itpflag, sitp.itp)
    newaxes = map(r->Base.Slice(ceil(Int, first(r)):floor(Int, last(r))), sitp.ranges)
    wis = dimension_wis(value_weights, itps, axes(sitp.itp), newaxes, sitp.ranges)
    wis1 = wis[1]
    i1 = first(axes(wis1, 1))
    breaks1 = [i1]
    for i in Iterators.drop(axes(wis1, 1), 1)
        if indexes(wis1[i]) != indexes(wis1[i-1])
            push!(breaks1, i)
        end
    end
    push!(breaks1, last(axes(wis1, 1))+1)
    ScaledIterator(sitp, CartesianIndices(newaxes), wis, breaks1)
end

function dimension_wis(f::F, itps, axs, newaxes, ranges) where F
    itpflag, ax, nax, r = itps[1], axs[1], newaxes[1], ranges[1]
    function makewi(x)
        pos, coefs = weightedindex_parts((f,), itpflag, ax, coordlookup(r, x))
        maybe_weightedindex(pos, coefs[1])
    end
    (makewi.(nax), dimension_wis(f, Base.tail(itps), Base.tail(axs), Base.tail(newaxes), Base.tail(ranges))...)
end
dimension_wis(f, ::Tuple{}, ::Tuple{}, ::Tuple{}, ::Tuple{}) = ()

function Base.iterate(iter::ScaledIterator)
    ret = iterate(iter.ci)
    ret === nothing && return nothing
    item, cistate = ret
    wis = getindex.(iter.wis, Tuple(item))
    ces = cache_evaluations(iter.sitp.itp.coefs, indexes(wis[1]), weights(wis[1]), Base.tail(wis))
    return _reduce(+, weights(wis[1]).*ces), ScaledIterState(cistate, first(iter.breaks1), ces)
end

function Base.iterate(iter::ScaledIterator, state)
    ret = iterate(iter.ci, state.cistate)
    ret === nothing && return nothing
    item, cistate = ret
    i1 = item[1]
    isnext1 = i1 == state.cistate[1]+1
    if isnext1 && i1 < iter.breaks1[state.ibreak+1]
        # We can use the previously cached values
        wis1 = iter.wis[1][i1]
        return _reduce(+, weights(wis1).*state.cached_evaluations), ScaledIterState(cistate, state.ibreak, state.cached_evaluations)
    end
    # Re-evaluate. We're being a bit lazy here: in some cases, some of the cached values could be reused
    wis = getindex.(iter.wis, Tuple(item))
    ces = cache_evaluations(iter.sitp.itp.coefs, indexes(wis[1]), weights(wis[1]), Base.tail(wis))
    return _reduce(+, weights(wis[1]).*ces), ScaledIterState(cistate, isnext1 ? state.ibreak+1 : first(iter.breaks1), ces)
end

_reduce(op, list) = op(list[1], _reduce(op, Base.tail(list)))
_reduce(op, list::Tuple{Number}) = list[1]
_reduce(op, list::Tuple{}) = error("cannot reduce an empty list")

# We use weights only as a ruler to determine when we are done
cache_evaluations(coefs, i::Int, weights, rest) = (coefs[i, rest...], cache_evaluations(coefs, i+1, Base.tail(weights), rest)...)
cache_evaluations(coefs, indexes, weights, rest) = (coefs[indexes[1], rest...], cache_evaluations(coefs, Base.tail(indexes), Base.tail(weights), rest)...)
cache_evaluations(coefs, ::Int, ::Tuple{}, rest) = ()
cache_evaluations(coefs, ::Any, ::Tuple{}, rest) = ()

ssize(sitp::ScaledInterpolation{T,N}) where {T,N} = map(r->round(Int, last(r)-first(r)+1), sitp.ranges)::NTuple{N,Int}


text:
abstract type DropRepeats <: PullType end
"""
    droprepeats(input)

Drop updates to `input` whenever the new value is the same
as the previous value of the signal.
"""
droprepeats(s) = Signal(x -> x, s; pull_type = DropRepeats)
export droprepeats

function (pa::PullAction{DropRepeats, A})(s) where A
    args = pull_args(pa)
    if !valid(s)
        old_val = value(s)
        res = pa.f(args...)
        if old_val == res
            validate(s)
        end
        store!(s, res)
    end
    value(s)
end

abstract type Filter <: PullType end
import Base.filter
"""
    filter(f, default, signal)

Remove updates from the `signal` where `f` does not return `true`. The filter will hold
the value default until f(value(signal)) returns true, when it will be updated
to value(signal).
"""
function filter(f::Function, v0, s::Signal)
    sd = SignalData(f(s[]) ? s[] : v0)
    action = PullAction(f, (s,), Filter)
    Signal(sd, action)
end

function (pa::PullAction{Filter, Tuple{Signal}})(s)
    source_val = pull!(pa.args[1])
    if !valid(s)
        if pa.f(source_val) === true
            store!(s, source_val)
        else
            validate(s)
        end
    end
    value(s)
end

abstract type When <: PullType end
"""
    when(f, condition::Signal, args...; v0 = nothing)

Create a `Signal` that will update to `f(args...)` when any of its input `args`
updates *only if* `condition` has value `true`. If `condition != true` in
the time of creation the signal will be initialized to value `v0`.

# Examples

    julia> A = Signal(1)
    julia> condition = Signal(A) do a
               a<10
           end
    julia> B = when(condition,A) do a
           println("\$a is smaller than 10")
       end
    julia> A(1)
    1 is smaller than 10
    1

    julia> A(12)
    12
"""
function when(f::Function, condition::Signal, args...; v0 = nothing)
    action = PullAction(f, args, When)
    sd = SignalData(condition() ? action() : v0)
    s = Signal(sd, action, condition)
    push!(condition.children, s) # update signal graph if condition changes to true
    s
end
export when

function (pa::PullAction{When, A})(s) where A
    condition = state(s)
    args = pull_args(pa)
    if !valid(s)
        if condition() === true
            store!(s, pa.f(args...))
        else
            validate(s)
        end
    end
    value(s)
end

"""
    sampleon(A, B)

Sample the value of `B` whenever `A` updates.
"""
function sampleon(A, B)
    Signal(x -> B(), A)
end
export sampleon

"""
fold over past values

    foldp(op, v0 ,sig)

reduce the given signal `sig` with the given binary operator `op`.
 the value of the signal just after creation is `op(v0,sig[])`
"""
function foldp(op::Function, v0, sig::Signal)
    acc = SignalData(op(v0, sig()))
    action = PullAction((acc, sig)) do acc, s
        op(value(acc), s)
    end
    Signal(acc, action)
end
export foldp

abstract type Merge <: PullType end
import Base.merge
"""
    merge(s::Signal,rest...)

Merge many signals into one. Returns a signal which updates when
any of the inputs update. If many signals update at the same time,
the value of the *last* non-valid(updated) input signal in the argument list is taken.
"""
function merge(in1::Signal, rest::Signal...)
    args = (in1, rest...)
    proxy_args = map(args) do arg
        Signal(x -> x, arg)
    end
    sd = SignalData(value(last(args)))
    action = PullAction(() -> nothing, proxy_args, Merge)
    Signal(sd, action)
end

function (pa::PullAction{Merge, A})(s) where A
    res = s[]
    for arg in pa.args
        if !valid(arg)
            pull!(arg)
            if !valid(s)
                res = arg[]
            end
        end
    end
    store!(s, res)
end

"""
    echo(s::Signal,name = "")

echos the value of signal `s` on each update, you can specify a `name` to distinguish between different echos
"""
function echo(s::Signal, name = "")
    Signal(x -> println("signal $name value : $x"), s)
end
export echo

abstract type RecursionFree <: PullType end
"""
    recursion_free(f::Function,args...)

creates a `Signal` whos action `f(args...)` is protected against infinite
loop recursion.

    julia> A = Signal(1)
    julia> B = recursion_free(A) do a
                A(a+1)
           end

    julia> A(10)
    10
    julia> A[]
    11
In the example above ,if `recursion_free` were to be subsituted
with regular `Signal` it would result in an infinite loop in the non-async mode
`recursion_free` protects against that

"""
function recursion_free(f::Function, args...)
    action = PullAction(f, args, RecursionFree)
    sd = SignalData(action())
    s = Signal(sd, action, false)
    for arg in args
        if typeof(arg) <: Signal
            #move to the top of the food chain
            pushfirst!(arg.children, pop!(arg.children))
        end
    end
end
export recursion_free

function (pa::PullAction{RecursionFree, A})(s) where A
    if s.state.x == true
        validate(s.data)
        validate(s)
    else
        s.state.x = true
        args = pull_args(pa)
        if !valid(s)
            store!(s, pa.f(args...))
        end
    end
    s.state.x = false
    value(s)
end

import Base.count
"""

    count_signal(s::Signal)
Create a `Signal` that counts updates to input `Signal` `s`
"""
function count(s::Signal)
    Signal(s; state = 0) do s, state
        state.x += 1
    end
end
export count

"""

    previous(s::Signal)
Create a `Signal` that holds previous input to `s`
"""
function previous(s::Signal)
    prev = Signal(s())
    Signal(s) do s
        ret = prev[]
        prev(s)
        ret
    end
end
export previous


text:
module FindSL

include("valuetostate.jl")
include("idMap.jl")
include("essential.jl")
include("expression.jl")
include("nlmodel.jl")
include("pi.jl")

function run(pifile, slfilename, lowerbound, upperbound, downregulatedcutoff, upregulatedcutoff, pairwisefile, dbidfile, tissueType, verbose)
    pinodes = Pi.readFile(pifile)
    IDMap = IdMap.get(dbidfile)

    expression = Expression.get(IDMap, tissueType)
    #essentialgenes = Essential.getGenes(collect(keys(pinodes)), dbidfile)
    essentialgenes = AbstractString["419195","5668934"]
    essentialgenes = Essential.getGenes(collect(keys(pinodes)), dbidfile)

    allGenes = DbIdNameMapping.allGeneReferenceProduct(dbidfile)
    allGenesSet = Set(allGenes)

    geneList = ["CIT_[cytosol]_53072",
        "DOCK1_[cytosol]_89242",
	"DOCK2_[cytosol]_89244",
	"ERBB2_[plasma_membrane]_54422",
	"HGF(32-494)_[extracellular_region]_56498",
	"HGF(495-728)_[extracellular_region]_56498",
	"KALRN_[cytosol]_401664",
	"MET_[plasma_membrane]_402617",
	"p-Y1234_Y1235_Y1349_Y1356-MET_[plasma_membrane]_402617",
	"p-Y1420-DCC_[plasma_membrane]_53436",
	"p-Y829-TIAM1_[cytosol]_195051",
	"TIAM1_[cytosol]_195051",
	"TRIO_[cytosol]_401916"]

    downregulatednodestates = []
    for node in keys(pinodes)
        if in(node, allGenesSet) == false
            continue
        end

        if pinodes[node].relation != "ROOT"
            continue
        end
        if contains(node, "PSEUDONODE") || in(node, Set(essentialgenes))
            continue
        end

        for i in [0,2]
            if in(node, Set(essentialgenes)) == false || i == 2
                (sampleresults, x, x_bar) = NLmodel.run(pinodes,
                                                        Dict(node => i),
                                                        essentialgenes,
                                                        lowerbound,
                                                        upperbound,
                                                        expression,
                                                        verbose)

                essentialnodes = []
                for resultnode in keys(sampleresults)
                    value = sampleresults[resultnode]
                    state = ValueToState.getState(value,
                                                  downregulatedcutoff,
                                                  upregulatedcutoff)

                    if state == "Down Regulated"
                        push!(essentialnodes, Dict("value" => value,
                                                   "name" => resultnode ))
                    end
                end
                if length(essentialnodes) != 0
                    nodestate = Dict("name" => node,
                                     "state" => i,
                                     "essentialNodes" => essentialnodes)
                    push!(downregulatednodestates, nodestate)
                end
            end
        end
    end

    sloutfile = open(slfilename, "w")
    write(sloutfile, "count\tnode_one\tstate\tnode_one_value\tnode_two\tstate\tnode_two_value\teffected_node\teffected_node_value\n")
    flush(sloutfile)

    count = 1
    candidates = 0
    index = 0
    indextwo = 0
    for nodeone in downregulatednodestates
        index = index + 1
        for nodetwo in downregulatednodestates
            indextwo = indextwo + 1
            if indextwo <= index
                continue
            end

            if in(nodeone["name"], geneList) == false && in(nodetwo["name"], geneList) == false
               continue
            end

            candidates = candidates + 1

            (sampleresults, x, x_bar) = NLmodel.run(pinodes,
                                            Dict(nodeone["name"] => nodeone["state"],
                                                 nodetwo["name"] => nodetwo["state"]),
                                                 essentialgenes,
                                                 lowerbound,
                                                 upperbound,
                                                 expression,
                                                verbose)
            for resultnode in keys(sampleresults)
               value = sampleresults[resultnode]
               state = ValueToState.getState(value,
                                        downregulatedcutoff,
                                        upregulatedcutoff)

               if state == "Down Regulated"
                   essentialnodesone = nodeone["essentialNodes"]
                   for essentialnodeone in essentialnodesone
                        if essentialnodeone["name"] != resultnode
                            continue
                        end
                        essentialnodestwo = nodetwo["essentialNodes"]
                        for essentialnodetwo in essentialnodestwo
                            if essentialnodetwo["name"] != resultnode
                                continue
                            end

                            nameone = essentialnodeone["name"]
                            valueone = essentialnodeone["value"]
                            nametwo = essentialnodetwo["name"]
                            valuetwo = essentialnodetwo["value"]

                            if (value < valueone) && (value < valuetwo)
                                write(sloutfile, "$count\t")
                                write(sloutfile, nodeone["name"])
                                write(sloutfile, "\t")
                                write(sloutfile, String(nodeone["state"]))
                                write(sloutfile, "\t")
                                write(sloutfile, string(valueone))
                                write(sloutfile, "\t")
                                write(sloutfile, nodetwo["name"])
                                write(sloutfile, "\t")
                                write(sloutfile, String(nodetwo["state"]))
                                write(sloutfile, "\t")
                                write(sloutfile, string(valuetwo))
                                write(sloutfile, "\t")
                                write(sloutfile, "$resultnode\t$value\n")
                                flush(sloutfile)

                            end
                            count = count + 1
                            break
                        end

                    end
                end
            end
        end
    end
    println("number of candidate SL $candidates\n")
    close(sloutfile)
end

end

max_stars_repo_name: OICR/mp-biopath

max_issues_repo_name: OICR/mp-biopath

max_forks_repo_name: OICR/mp-biopath


text:
# This function evaluates and returns the coefficients for constraints and cost which are used later in the MPC formulation
# Inputs are:
# oldTraj   -> contains information about previous trajectories and Inputs
# mpcCoeff  -> contains information about previous coefficient results
# posInfo   -> contains information about the car's current position along the track
# mpcParams -> contains information about the MPC formulation (e.g. Q, R)
# stateIn   -> current state of the car
# inputIn   -> last input to the system

# structure of oldTrajectory: 1st dimension = state number, 2nd dimension = step number (time equiv.), 3rd dimennsion = lap number

# z[1] = xDot
# z[2] = yDot
# z[3] = psiDot
# z[4] = ePsi
# z[5] = eY
# z[6] = s

function coeffConstraintCost(oldTraj::OldTrajectory, mpcCoeff::MpcCoeff, posInfo::PosInfo, mpcParams::MpcParams,lapStatus::LapStatus,
                             selectedStates::SelectedStates,oldSS::SafeSetData,obs::Array{Float64},obstacle::Obstacle)
    # this computes the coefficients for the cost and constraints

    # Outputs:
    # coeffConst
    # coeffCost

    # Read Inputs
    s               = posInfo.s
    s_target        = posInfo.s_target

    dt = 0.1
    N  = mpcParams.N

    Np              = selectedStates.Np
    Nl              = selectedStates.Nl

    # Parameters
    Order           = mpcCoeff.order                # interpolation order for cost and constraints
    pLength         = mpcCoeff.pLength              # interpolation length for polynomials
    delay_df        = mpcParams.delay_df
    #delay           = 2

    #coeffCost       = zeros(Order+1,2)              # polynomial coefficients for cost
    #coeffConst      = zeros(Order+1,2,5)            # nz-1 beacuse no coeff for s

    n_laps_sysID    = 2                             # number of previous laps that are used for sysID

    selected_laps = zeros(Int64,Nl)
    # selected_laps[1] = lapStatus.currentLap-1                                   # use previous lap
    # selected_laps[2] = lapStatus.currentLap-2                                   # and the one before
    # if lapStatus.currentLap >= 5
    #     selected_laps[2] = indmin(oldTraj.oldCost[2:lapStatus.currentLap-2])+1      # and the best from all previous laps
    # end

    for i = 1:Nl
        selected_laps[i] = lapStatus.currentLap-i    # use previous lap
    end

    if lapStatus.currentLap >= 5
        selected_laps[Nl] = indmin(oldSS.oldCost[1:lapStatus.currentLap-2])      # and the best from all previous laps
    end

    # Select the old data
    oldxDot         = oldTraj.oldTraj[:,1,selected_laps]::Array{Float64,3}
    oldyDot         = oldTraj.oldTraj[:,2,selected_laps]::Array{Float64,3}
    oldpsiDot       = oldTraj.oldTraj[:,3,selected_laps]::Array{Float64,3}
    oldePsi         = oldTraj.oldTraj[:,4,selected_laps]::Array{Float64,3}
    oldeY           = oldTraj.oldTraj[:,5,selected_laps]::Array{Float64,3}
    oldS            = oldTraj.oldTraj[:,6,selected_laps]::Array{Float64,3}
    oldacc          = oldTraj.oldTraj[:,7,selected_laps]::Array{Float64,3}
    olda            = oldTraj.oldInput[:,1,selected_laps]::Array{Float64,3}
    olddF           = oldTraj.oldInput[:,2,selected_laps]::Array{Float64,3}
    #olddF           = smooth(olddF,5)

    oldS_safeSet    = oldSS.oldSS[:,6,selected_laps]::Array{Float64,3}
    #println("safe set= ",oldS_safeSet[1:20,:,:])

    N_points        = size(oldTraj.oldTraj,1)     # second dimension = length (=buffersize)
    N_points2       = size(oldSS.oldSS,1)

    s_total::Float64        # initialize
    DistS::Array{Float64}   # initialize
    idx_s::Array{Int64}     # initialize
    idx_s_target        = 0
    dist_to_s_target    = 0
    qLength             = 0
    vec_range::Tuple{UnitRange{Int64},UnitRange{Int64}}
    bS_Vector::Array{Float64}
    s_forinterpy::Array{Float64}

    # Compute the total s (current position along track)
    s_total = s % s_target

    # Compute the index
    DistS = ( s_total - oldS ).^2
    DistS2 = ( s_total - oldS_safeSet ).^2

    idx_s = findmin(DistS,1)[2]              # contains both indices for the closest distances for both oldS !!
    idx_s2= findmin(DistS2,1)[2]

    idx_s2 = idx_s2 + selectedStates.shift

    # Propagate the obstacle for the prediction horizon

    obs_prop_s  = obs[1,1,:] + dt*N*obs[1,3,:]
    obs_prop_ey = obs[1,2,:]

    #######################################################################

    for j = 0:(Nl-1)

        selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,m=1:6] = oldSS.oldSS[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]+Np-(j*N_points2)-1,i=1:6,selected_laps[j+1]]  # select the states from lap j...

        selectedStates.statesCost[i=(j*Np)+1:(j+1)*Np] = oldSS.cost2target[i=idx_s2[j+1]-(j*N_points2):idx_s2[j+1]-(j*N_points2)+Np-1,selected_laps[j+1]]  # and their cost

        if obstacle.obstacle_active == true   # if the obstacles are on the track, check if any of the selected states interferes with the propagated obstacle
            for n=1:obstacle.n_obs
                ellipse_check = (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,6]-obs_prop_s[n])/obstacle.r_s).^2) + (((selectedStates.selStates[i=(j*Np)+1:(j+1)*Np,5]-obs_prop_ey[n])/obstacle.r_ey).^2)

                if any(x->x<=1, ellipse_check) == true  # if any of the selected states is in the ellipse
                    #println("flag**************************************************************************************************************")
                    index = find(ellipse_check.<=1)     # find all the states in the ellipse

                    mpcParams.Q_obs[i=(j*Np)+(index[1]-obstacle.inv_step)+1:(j+1)*Np] =  10   # and set the values of the weight to 10, so that they are excluded from optimization
                    println("Q_obs= ",mpcParams.Q_obs)
                end
            end
        end

    end

        #println("Q_obs= ",mpcParams.Q_obs)

    ##########################################################################
    if selectedStates.version == true
        vec_range = (idx_s[1]:idx_s[1]+pLength,idx_s[2]:idx_s[2]+pLength)

        # Create the vectors used for the interpolation
        # bS_vector contains the s-values for later interpolation
        bS_Vector       = zeros(pLength+1,2)
        for i=1:pLength+1
            bS_Vector[i,1] = oldS[vec_range[1][i]]
            bS_Vector[i,2] = oldS[vec_range[2][i]]
        end
        # if norm(bS_Vector[1,1]-s_total) > 0.3 || norm(bS_Vector[1,2]-s_total) > 0.3
        #     warn("Couldn't find a close point to current s.")
        # end

        # The states are parametrized with resprect to the curvilinear abscissa,
        # so we select the point used for the interpolation. Need to subtract an
        # offset to be coherent with the MPC formulation
        s_forinterpy   = bS_Vector
        if s_total < 0
            s_forinterpy += s_target
        end

        # Create the Matrices for the interpolation
        MatrixInterp = zeros(pLength+1,Order+1,2)

        for k = 0:Order
            MatrixInterp[:,Order+1-k,:] = s_forinterpy[:,:].^k
        end

        # Compute the coefficients
        #coeffConst = zeros(Order+1,2,5)
        for i=1:2
            mpcCoeff.coeffConst[:,i,1]    = MatrixInterp[:,:,i]\oldxDot[vec_range[i]]
            mpcCoeff.coeffConst[:,i,2]    = MatrixInterp[:,:,i]\oldyDot[vec_range[i]]
            mpcCoeff.coeffConst[:,i,3]    = MatrixInterp[:,:,i]\oldpsiDot[vec_range[i]]
            mpcCoeff.coeffConst[:,i,4]    = MatrixInterp[:,:,i]\oldePsi[vec_range[i]]
            mpcCoeff.coeffConst[:,i,5]    = MatrixInterp[:,:,i]\oldeY[vec_range[i]]
        end

        # Finished with calculating the constraint coefficients

        # Now compute the final cost coefficients

        # The Q-function contains for every point in the sampled safe set the minimum cost-to-go-value
        # These values are calculated for both old trajectories
        # The vector bQfunction_Vector contains the cost at each point in the interpolated area to reach the finish line
        # From this vector, polynomial coefficients coeffCost are calculated to approximate this cost
        for i=1:2
                dist_to_s_target  = oldTraj.oldCost[selected_laps[i]] + oldTraj.idx_start[selected_laps[i]] - (idx_s[i]-N_points*(i-1))  # number of iterations from idx_s to s_target
                bQfunction_Vector = collect(linspace(dist_to_s_target,dist_to_s_target-pLength,pLength+1))    # build a vector that starts at the distance and
                                                                                # decreases in equal steps
                mpcCoeff.coeffCost[:,i]    = MatrixInterp[:,:,i]\bQfunction_Vector           # interpolate this vector with the given s
        end
    end

    # --------------- SYSTEM IDENTIFICATION --------------- #
    # ----------------------------------------------------- #

    cC = oldTraj.count[lapStatus.currentLap]-1      # current count
    cL = lapStatus.currentLap                       # current lap
    #vec_range_ID2   = cC-n_prev:cC-1                # index range for current lap

    # TODO:*******************
    # CHECK IF DIFFERENCES ARE ALIGNED PROPERLY
    # ADD 2-step-delay to sysID
    # ************************
    # collect indices for system ID
    n_sys_ID_prev = 60              # steps of sysID before current point in previous laps
    n_sys_ID_post = 60              # steps of sysID after current point in previous laps
    n_sys_ID_prev_c = 30            # steps of sysID before current point in current lap

    freq_ratio = 5                  # ratio between the frequency at which the informations about the position are updated in the topic and the frequency at which the controller calculates and updates the control input
    # vec_range_ID    = ()
    # for i=1:n_laps_sysID
    #     vec_range_ID    = tuple(vec_range_ID...,idx_s[i]-n_prev:idx_s[i]+n_ahead)     # related index range
    # end

    sysID_idx_diff = idx_s[1]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio)         # these are the indices that are used for differences
    sysID_idx = sysID_idx_diff[1:end-1]

    sysID_idx_diff2 = idx_s[2]-n_sys_ID_prev*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev+n_sys_ID_post+1)*freq_ratio)         # these are the indices that are used for differences
    sysID_idx2 = sysID_idx_diff2[1:end-1]

    sysID_idx_diff_c = cC - (n_sys_ID_prev_c+1)*freq_ratio-1 + (1:freq_ratio:(n_sys_ID_prev_c+1)*freq_ratio)
    sysID_idx_c = sysID_idx_diff_c[1:end-1]

    sz1 = size(sysID_idx,1)
    sz2 = size(sysID_idx_c,1)
    sz2 = 0
    sz = sz1 + sz2

    # psiDot
    y_psi = zeros((2*sz1+sz2)*freq_ratio)
    A_psi = zeros((2*sz1+sz2)*freq_ratio,3)
    for i=0:freq_ratio-1
        y_psi[(1:sz1)+i*sz1]            = diff(oldpsiDot[sysID_idx_diff+i])
        A_psi[(1:sz1)+i*sz1,:]          = [oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i]]
        #y_psi[(1:sz2)+i*sz2+freq_ratio*sz1]      = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,3,cL])
        #A_psi[(1:sz2)+i*sz2+freq_ratio*sz1,:]    = [oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2,cL]]
        y_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2]   = diff(oldpsiDot[sysID_idx_diff2+i])
        A_psi[(1:sz1)+i*sz1+freq_ratio*sz1+freq_ratio*sz2,:] = [oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i]]
    end

    # xDot
    y_xDot = zeros((2*sz1+sz2)*freq_ratio)
    A_xDot = zeros((2*sz1+sz2)*freq_ratio,3)
    for i=0:freq_ratio-1
        y_xDot[(1:sz1)+i*sz1]            = diff(oldxDot[sysID_idx_diff+i])
        A_xDot[(1:sz1)+i*sz1,:]          = [oldyDot[sysID_idx+i].*oldpsiDot[sysID_idx+i] oldxDot[sysID_idx+i] oldacc[sysID_idx+i]]
        #y_xDot[(1:sz2)+i*sz2+freq_ratio*sz1]      = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,1,cL])
        #A_xDot[(1:sz2)+i*sz2+freq_ratio*sz1,:]    = [oldTraj.oldTraj[sysID_idx_c+i,2,cL].*oldTraj.oldTraj[sysID_idx_c+i,3,cL] oldTraj.oldTraj[sysID_idx_c+i,1,cL]  oldTraj.oldTraj[sysID_idx_c+i,7,cL]]
        y_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)]   = diff(oldxDot[sysID_idx_diff2+i])
        A_xDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i].*oldpsiDot[sysID_idx2+i] oldxDot[sysID_idx2+i] oldacc[sysID_idx2+i]]
    end

    # yDot
    y_yDot = zeros((2*sz1+sz2)*freq_ratio)
    A_yDot = zeros((2*sz1+sz2)*freq_ratio,4)
    for i=0:freq_ratio-1
        y_yDot[(1:sz1)+i*sz1]            = diff(oldyDot[sysID_idx_diff+i])
        A_yDot[(1:sz1)+i*sz1,:]          = [oldyDot[sysID_idx+i]./oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i].*oldxDot[sysID_idx+i] oldpsiDot[sysID_idx+i]./oldxDot[sysID_idx+i] olddF[sysID_idx+i-delay_df*freq_ratio]]
        #y_yDot[(1:sz2)+i*sz2+freq_ratio*sz1]      = diff(oldTraj.oldTraj[sysID_idx_diff_c+i,2,cL])
        #A_yDot[(1:sz2)+i*sz2+freq_ratio*sz1,:]    = [oldTraj.oldTraj[sysID_idx_c+i,2,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL].*oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldTraj[sysID_idx_c+i,3,cL]./oldTraj.oldTraj[sysID_idx_c+i,1,cL] oldTraj.oldInput[sysID_idx_c+i-delay_df*freq_ratio,2]]
        y_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2)]   = diff(oldyDot[sysID_idx_diff2+i])
        A_yDot[(1:sz1)+i*sz1+freq_ratio*(sz1+sz2),:] = [oldyDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i].*oldxDot[sysID_idx2+i] oldpsiDot[sysID_idx2+i]./oldxDot[sysID_idx2+i] olddF[sysID_idx2+i-delay_df*5]]
    end

    # if any(isnan,y_yDot)            # check if any value in the y_yDot value is NaN
    #     println(y_yDot)
    #     warn("NaN value detected in y_yDot! Press to continue...")
    # end
    # if any(isnan,coeffCost)
    #     println(coeffCost)
    #     warn("NaN value detected in coeffCost! Press to continue...")
    # end
    # if any(isnan,coeffConst)
    #     println(coeffCost)
    #     warn("NaN value detected in coeffConst! Press to continue...")
    # end

    # mpcCoeff.c_Psi = zeros(3)
    # mpcCoeff.c_Vx = zeros(4)
    # mpcCoeff.c_Vy = zeros(4)

    mpcCoeff.c_Psi = (A_psi'*A_psi)\A_psi'*y_psi
    mpcCoeff.c_Vx  = (A_xDot'*A_xDot)\A_xDot'*y_xDot         # the identity matrix is used to scale the coefficients
    mpcCoeff.c_Vy  = (A_yDot'*A_yDot)\A_yDot'*y_yDot

    # if det(A_psi'*A_psi) != 0
    # else
    #     println("det y_psi = 0")
    # end
    # if det(A_xDot'*A_xDot) != 0
    # else
    #     println("det vx = 0")
    # end
    # if det(A_yDot'*A_yDot) != 0
    # else
    #     println("det vy = 0")
    # end
    #println("Done. c_Psi = $(mpcCoeff.c_Psi)")
    #mpcCoeff.c_Psi = [-1.0,-4.5,5.8]
    #mpcCoeff.c_Vy  = [-1.2,-0.04,-0.01,0.6]
    #mpcCoeff.c_Psi = [-0.26682109207165566,-0.013445078992161885,1.2389672517023724]
    #mpcCoeff.c_Vy = [-0.006633028965076818,-0.02997779668710061,0.005781203137095575,0.10642934131787765]
    #mpcCoeff.coeffCost  = coeffCost
    #mpcCoeff.coeffConst = coeffConst
    nothing
end

# Notes about oldTrajectory:
# oldTrajectory[1:prebuf] = states before finish line (s < 0)
# oldTrajectory[prebuf+1:prebuf+cost] = states between start and finish line (0 <= s < s_target)
# oldTrajectory[prebuf+cost+1:prebuf+cost+postbuf] = states after finish line (s_target <= s)
# once one lap is over, the states are saved (interval prebuf:s_target)
# during the next lap, the states behind the finish line are appended (interval s_target:postbuf)

max_stars_repo_name: RoyaFiroozi/barc

max_issues_repo_name: RoyaFiroozi/barc

max_forks_repo_name: RoyaFiroozi/barc