text:
export get_ts_s;
export get_units_by_kind;
export get_units_by_bus;
export get_bus;
export sc_opf;
export print_nz;

using Xpress;

function get_bus(launcher::Launcher, names::Set{String})
    result = Set{String}();
    for name in names
        if !haskey(launcher.gen_type_bus, name)
            push!(result, name);
        end
    end
    return result;
end

function get_ech_ts_s_name(launcher::Launcher)
    ech_set = Set{DateTime}();
    ts_set = Set{DateTime}();
    s_set =  Set{String}();
    name_set = Set{String}();
    for uncertainty in launcher.uncertainties
        ts = uncertainty[1][U_H];
        ech = uncertainty[1][U_ECH];
        s = uncertainty[1][U_SCENARIO];
        name = uncertainty[1][U_NAME];
        push!(ech_set, ech);
        push!(ts_set, ts);
        push!(s_set, s);
        push!(name_set, name);
    end
    return sort(collect(ech_set)), sort(collect(ts_set)), sort(collect(s_set)), name_set;
end

function get_units_by_kind(launcher::Launcher)
    result = Dict{String,Dict{String,String}}();
    result[K_LIMITABLE] = Dict{String,String}();
    result[K_IMPOSABLE] = Dict{String,String}();
    for gen in launcher.gen_type_bus
        kind = gen[2][1];
        push!(result[kind],  gen[1] => gen[2][2]);
    end
    return result;
end

function get_units_by_bus(launcher::Launcher, buses::Set{String})
    result = Dict{String,Dict{String,Vector{String}}}();
    result[K_LIMITABLE] = Dict{String,Vector{String}}([bus => Vector{String}() for bus in buses]);
    result[K_IMPOSABLE] = Dict{String,Vector{String}}([bus => Vector{String}() for bus in buses]);
    for gen in launcher.gen_type_bus
        name = gen[1];
        kind = gen[2][1];
        bus  = gen[2][2];
        tmp = get(result[kind], bus, Vector{String}())
        push!(tmp, name);
        result[kind][bus] = tmp;
    end
    return result;
end
function add_limitable!(launcher::Launcher, ech::DateTime, model, units_by_kind, TS, S)
    p_lim = Dict{Tuple{String,DateTime},VariableRef}();
    is_limited = Dict{Tuple{String,DateTime,String},VariableRef}();
    p_enr = Dict{Tuple{String,DateTime,String},VariableRef}();
    is_limited_x_p_lim = Dict{Tuple{String,DateTime,String},VariableRef}();
    c_lim = Dict{Tuple{String,DateTime,String},VariableRef}();
    if ! launcher.NO_LIMITABLE
        for kvp in units_by_kind[K_LIMITABLE], ts in TS
            gen = kvp[1];
            p_lim[gen, ts] =  @variable(model, base_name = @sprintf("p_lim[%s,%s]", gen, ts), lower_bound = 0);
            pLimMax = 0;
            for s in S
                pLimMax = max(pLimMax, launcher.uncertainties[gen, s, ts, ech]);
            end
            for s in S
                name =  @sprintf("is_limited[%s,%s,%s]", gen, ts, s);
                is_limited[gen, ts, s] = @variable(model, base_name = name, binary = true);

                name =  @sprintf("is_limited_x_p_lim[%s,%s,%s]", gen, ts, s);
                is_limited_x_p_lim[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0);

                name =  @sprintf("c_lim[%s,%s,%s]", gen, ts, s);
                c_lim[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0);

                @constraint(model, is_limited_x_p_lim[gen, ts, s] <= p_lim[gen, ts]);
                @constraint(model, is_limited_x_p_lim[gen, ts, s] <= is_limited[gen, ts, s] * pLimMax);
                @constraint(model, is_limited_x_p_lim[gen, ts, s] + pLimMax * (1 - is_limited[gen, ts, s]) - p_lim[gen, ts] >= 0);

                p0 = launcher.uncertainties[gen, s, ts, ech];
                name =  @sprintf("p_enr[%s,%s,%s]", gen, ts, s);
                p_enr[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0, upper_bound = p0);
                @constraint(model, p_enr[gen, ts, s] == (1 - is_limited[gen, ts, s]) * p0 + is_limited_x_p_lim[gen, ts, s]);
                @constraint(model, p_enr[gen, ts, s] <= p_lim[gen, ts]);

                @constraint(model, c_lim[gen, ts, s] >= p0 - p_lim[gen, ts]);

                if launcher.NO_LIMITATION
                    @constraint(model, is_limited[gen, ts, s] == 0);
                end
            end
        end
    end
    return Workflow.LimitableModeler(p_lim, is_limited, p_enr, is_limited_x_p_lim, c_lim);
    # return p_lim, is_limited, is_limited_x_p_lim, c_lim;
end

function add_imposable!(launcher::Launcher, ech, model,  units_by_kind, TS, S)
    p_imposable = Dict{Tuple{String,DateTime,String},VariableRef}();
    p_is_imp = Dict{Tuple{String,DateTime},VariableRef}();
    p_is_imp_and_on = Dict{Tuple{String,DateTime},VariableRef}();
    p_imp = Dict{Tuple{String,DateTime},VariableRef}();
    p_start = Dict{Tuple{String,DateTime},VariableRef}();
    p_on = Dict{Tuple{String,DateTime},VariableRef}();
    c_imp_pos  = Dict{Tuple{String,DateTime,String},VariableRef}();
    c_imp_neg   = Dict{Tuple{String,DateTime,String},VariableRef}();
    if ! launcher.NO_IMPOSABLE
        for kvp in units_by_kind[K_IMPOSABLE]
            gen = kvp[1];
            for ts in TS
                pMin = launcher.units[gen][1];
                pMax = launcher.units[gen][2];

                name =  @sprintf("p_imp[%s,%s]", gen, ts);
                p_imp[gen, ts] = @variable(model, base_name = name);

                name =  @sprintf("p_is_imp[%s,%s]", gen, ts);
                p_is_imp[gen, ts] = @variable(model, base_name = name, binary = true);

                name =  @sprintf("p_start[%s,%s]", gen, ts);
                p_start[gen, ts] = @variable(model, base_name = name, binary = true);

                name =  @sprintf("p_on[%s,%s]", gen, ts);
                p_on[gen, ts] = @variable(model, base_name = name, binary = true);

                name =  @sprintf("p_is_imp_and_on[%s,%s]", gen, ts);
                p_is_imp_and_on[gen, ts] = @variable(model, base_name = name, binary = true);

                # if in(gen, ["park_city", "alta", "sundance"])
                #     println("WARNING");
                #     @constraint(model,  p_on[gen, ts]==0);
                #     @constraint(model,  p_is_imp[gen, ts]==0);
                # end
                for s in S
                    p0 = launcher.uncertainties[gen, s, ts, ech];
                    name =  @sprintf("p_imposable[%s,%s,%s]", gen, ts, s);
                    p_imposable[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0);

                    name =  @sprintf("c_imp_pos[%s,%s,%s]", gen, ts, s);
                    c_imp_pos[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0);

                    name =  @sprintf("c_imp_neg[%s,%s,%s]", gen, ts, s);
                    c_imp_neg[gen, ts, s] = @variable(model, base_name = name, lower_bound = 0);

                    @constraint(model, p_imposable[gen, ts, s] == p0 + c_imp_pos[gen, ts, s] - c_imp_neg[gen, ts, s]);

                    @constraint(model, p_imposable[gen, ts, s] == (1 - p_is_imp[gen, ts]) * p0 + p_imp[gen, ts]);
                    @constraint(model, p_imp[gen, ts] <= pMax * p_is_imp_and_on[gen, ts]);
                    @constraint(model, p_imp[gen, ts] >= pMin * p_is_imp_and_on[gen, ts]);

                    @constraint(model, p_is_imp_and_on[gen, ts] <= p_is_imp[gen, ts]);
                    @constraint(model, p_is_imp_and_on[gen, ts] <= p_on[gen, ts]);
                    @constraint(model, 1 + p_is_imp_and_on[gen, ts] >= p_on[gen, ts] + p_is_imp[gen, ts]);
                end
            end
            i = 0;
            for ts in TS
                i += 1;
                if i > 1
                    ts_1 = TS[i - 1];
                    @constraint(model, p_start[gen, ts] <= p_on[gen, ts]);
                    @constraint(model, p_start[gen, ts] <= 1 - p_on[gen, ts_1]);
                    @constraint(model, p_start[gen, ts] >= p_on[gen, ts] - p_on[gen, ts_1]);
                else
                    prev0 = launcher.previsions[gen, ts, ech];
                    println(@sprintf("%s %s is at %f\n", gen, ts, prev0));
                    if abs(prev0) < 1
                        @constraint(model, p_start[gen, ts] == p_on[gen, ts]);
                    end
                end
            end
        end
    end
    return Workflow.ImposableModeler(p_imposable, p_is_imp, p_imp, p_is_imp_and_on, p_start, p_on, c_imp_pos, c_imp_neg);
end

function add_eod_constraint!(launcher::Launcher, ech, model, units_by_bus, TS, S, BUSES, v_lim::Workflow.LimitableModeler, v_imp::Workflow.ImposableModeler, v_res::Workflow.ReserveModeler, netloads )
    eod_expr = Dict([(ts, s) => AffExpr(0) for ts in TS, s in S]);
    for ts in TS, s in S
        for bus in BUSES
            if ! launcher.NO_IMPOSABLE
                for gen in units_by_bus[K_IMPOSABLE][bus]
                    eod_expr[ts, s] += v_imp.p_imposable[gen, ts, s];
                end
            end
            if ! launcher.NO_LIMITABLE
                for gen in units_by_bus[K_LIMITABLE][bus]
                    p0 = launcher.uncertainties[gen, s, ts, ech];
                    # println(gen, " ", ts, " ", s, " ", p0);
                    eod_expr[ts, s] +=  ((1 -  v_lim.is_limited[gen, ts, s]) * p0 + v_lim.is_limited_x_p_lim[gen, ts, s]);
                end
            end
        end
    end
    for ts in TS, s in S
        eod_expr[ts, s] += v_res.p_res_pos[ts, s];
        eod_expr[ts, s] -= v_res.p_res_neg[ts, s];
    end

    # println(launcher.uncertainties)
    for ts in TS, s in S
        load = 0;
        for bus in BUSES
            load += netloads[bus, ts, s];
        end
        @constraint(model, eod_expr[ts, s] == load);
    end
end

function add_reserve!(launcher::Launcher, ech, model, TS, S, p_res_min, p_res_max)
    p_res_pos = Dict{Tuple{DateTime,String},VariableRef}();
    p_res_neg = Dict{Tuple{DateTime,String},VariableRef}();
    for ts in TS, s in S
        name =  @sprintf("p_res_pos[%s,%s]", ts, s);
        p_res_pos[ts, s] = @variable(model, base_name = name, lower_bound = 0, upper_bound = p_res_max);
        name =  @sprintf("p_res_neg[%s,%s]", ts, s);
        p_res_neg[ts, s] = @variable(model, base_name = name, lower_bound = 0, upper_bound = -p_res_min);
    end
    return Workflow.ReserveModeler(p_res_pos, p_res_neg);
end

function add_obj!(launcher::Launcher, model, TS, S, v_lim::Workflow.LimitableModeler, v_imp::Workflow.ImposableModeler, v_res::Workflow.ReserveModeler)
    eod_obj = AffExpr(0);
    w_lim = 1 / length(S);
    for x in v_lim.c_lim
        gen = x[1][1];
        cProp = launcher.units[gen][4];
        eod_obj += w_lim * cProp * x[2];
    end
    if length(v_lim.is_limited) > 0
        eod_obj += sum(1e-4 * x[2] for x in v_lim.is_limited);
    end
    for x in v_imp.p_start
        gen = x[1][1];
        cStart = launcher.units[gen][3];
        eod_obj += cStart * x[2];
    end
    for x in v_imp.c_imp_pos
        gen = x[1][1];
        cProp = launcher.units[gen][4];
        eod_obj += w_lim * cProp * x[2];
    end
    for x in v_imp.c_imp_neg
        gen = x[1][1];
        cProp = launcher.units[gen][4];
        eod_obj += w_lim * cProp * x[2];
    end
    for x in v_res.p_res_pos
        eod_obj += (1e-3)*w_lim*x[2];
    end
    for x in v_res.p_res_neg
        eod_obj += (1e-3)*w_lim* x[2];
    end
    @objective(model, Min, eod_obj);
end

function add_flow!(launcher::Workflow.Launcher, model, TS, S,  units_by_bus, v_lim::Workflow.LimitableModeler, v_imp::Workflow.ImposableModeler, netloads)
    v_flow = Dict{Tuple{String,DateTime,String},VariableRef}();

    ptdf_by_branch = Dict{String,Dict{String,Float64}}() ;
    for ptdf in launcher.ptdf
        branch, bus = ptdf[1]
        if ! haskey(ptdf_by_branch, branch)
            ptdf_by_branch[branch] = Dict{String,Float64}();
        end
        ptdf_by_branch[branch][bus] = ptdf[2];
    end
    println("ptdf_by_branch : ", ptdf_by_branch);
    for ts in TS, s in S
        for limit in launcher.limits
            branch = limit[1];
            v = limit[2]
            name =  @sprintf("p_flow[%s, %s,%s]", branch, ts, s);
            v_flow[branch, ts, s] = @variable(model, base_name = name, lower_bound = -v, upper_bound = v);
            f_ref = 0;
            xpr = AffExpr(0);
            for (bus, ptdf) in ptdf_by_branch[branch]
                f_ref -= ptdf * netloads[bus, ts, s];
                if ! launcher.NO_IMPOSABLE
                    for gen in units_by_bus[K_IMPOSABLE][bus]
                        xpr += v_imp.p_imposable[gen, ts, s] * ptdf;
                    end
                end
                if ! launcher.NO_LIMITABLE
                    for gen in units_by_bus[K_LIMITABLE][bus]
                        xpr += v_lim.p_enr[gen, ts, s] * ptdf;
                    end
                end
            end
            @constraint(model, v_flow[branch, ts, s] == f_ref + xpr);
        end
    end
    return v_flow;
end

function sc_opf(launcher::Launcher, ech::DateTime, p_res_min, p_res_max)
    ##############################################################
    ### optimisation modelling sets
    ##############################################################
    ECH, TS, S, NAMES = Workflow.get_ech_ts_s_name(launcher);
    BUSES =  Workflow.get_bus(launcher, NAMES);
    units_by_kind = Workflow.get_units_by_kind(launcher);
    units_by_bus =  Workflow.get_units_by_bus(launcher, BUSES);

    println("NAMES : ", NAMES);
    println("BUSES : ", BUSES);
    println("units_by_bus : ", units_by_bus);

    println("Number of scenario ", length(S));
    println("Number of time step is ", length(TS));
    println(ECH);
    K_IMPOSABLE = Workflow.K_IMPOSABLE;
    K_LIMITABLE = Workflow.K_LIMITABLE;

    netloads = Dict([(bus, ts, s) => launcher.uncertainties[bus, s, ts, ech] for bus in BUSES, ts in TS, s in S]);
    eod_slack = Dict([(ts,s) => 0.0 for s in S, ts in TS]);
    factor = Dict([name => 1 for name in NAMES]);
    for bus in BUSES
        factor[bus] = -1;
    end
    for ((name, s, ts, ech), v) in launcher.uncertainties
        eod_slack[ts,  s] += factor[name] * v;
    end
    println("eod_slack is $eod_slack");
    ##############################################################
    # to be in a function ...
    ##############################################################

    model = Model();
    v_lim = add_limitable!(launcher, ech, model, units_by_kind, TS, S);
    p_lim = v_lim.p_lim;
    is_limited = v_lim.is_limited;
    is_limited_x_p_lim = v_lim.is_limited_x_p_lim;
    c_lim = v_lim.c_lim;

    v_imp = add_imposable!(launcher, ech, model, units_by_kind, TS, S);

    p_imposable = v_imp.p_imposable;
    p_is_imp = v_imp.p_is_imp;
    p_imp = v_imp.p_imp;
    p_start = v_imp.p_start;
    p_on = v_imp.p_on;

    v_res = add_reserve!(launcher, ech, model, TS, S, p_res_min, p_res_max);

    add_eod_constraint!(launcher, ech, model, units_by_bus, TS, S, BUSES, v_lim, v_imp, v_res, netloads);

    v_flow = add_flow!(launcher, model, TS, S, units_by_bus,  v_lim, v_imp, netloads);

    # println(units_by_bus)
    # println(eod_expr)
    add_obj!(launcher,  model, TS, S, v_lim, v_imp, v_res);

    # println(model)
    set_optimizer(model, Xpress.Optimizer);
    optimize!(model);

    # print_nz(p_imposable);
    # print_nz(p_is_imp);
    # print_nz(p_start);
    # print_nz(p_on);
    # print_nz(is_limited);

    println("NO_IMPOSABLE   : ", launcher.NO_IMPOSABLE);
    println("NO_LIMITABLE   : ", launcher.NO_LIMITABLE);
    println("NO_LIMITATION  : ", launcher.NO_LIMITATION);
    println("BUSES          : ", BUSES);
    println("NAMES          : ", NAMES);
    println("TS             : ", TS);
    println("S              : ", S);

    @printf("%30s[%s,%s] %4s %10s %10s %10s\n", "gen", "ts", "s", "b_enr", "p_enr", "p_lim_enr", "p0");
    if ! launcher.NO_LIMITABLE
        # println(launcher.uncertainties)
        for bus in BUSES, ts in TS, s in S, gen in units_by_bus[K_LIMITABLE][bus]
            p0 = launcher.uncertainties[gen, s, ts, ech];
            b_enr = value(is_limited[gen, ts, s]);
            p_enr = value((1 - is_limited[gen, ts, s]) * p0 + is_limited_x_p_lim[gen, ts, s]);
            p_lim_enr = value(p_lim[gen, ts]);
            # println(value(is_limited_x_p_lim[gen, ts, s]) - b_enr * p_lim_enr)
            if b_enr > 0.5
                @printf("%10s[%s,%s] %4d %10.3f %10.3f %10.3f\n", gen, ts, s, b_enr, p_enr, p_lim_enr, p0);
            end
            # println(gen, " : ", value((1 - is_limited[gen, ts, s]) * p0 + is_limited_x_p_lim[gen, ts, s]))
        end
    end
    if ! launcher.NO_IMPOSABLE
        # println(launcher.uncertainties)
        for bus in BUSES, ts in TS, gen in units_by_bus[K_IMPOSABLE][bus]
            b_start = value(p_start[gen, ts]);
            b_imp = value(p_is_imp[gen, ts]);
            if b_imp > 0.5
                @printf("%10s[%s] IMPOSED\n", gen, ts);
            end
            if b_start > 0.5
                @printf("%10s[%s] STARTED\n", gen, ts);
            end
            b_on = value(p_on[gen, ts]);
            for s in S
                if b_on > 0.5
                    @printf("%10s[%s] %10.3f\n", gen, ts, value(v_imp.p_imposable[gen, ts, s]));
                end
            end

        end
    end
    println("v_res.p_res_pos");
    print_nz(v_res.p_res_pos);
    println("v_res.p_res_neg");
    print_nz(v_res.p_res_neg);
    println("v_res.v_flow");
    print_nz(v_flow);
    write_output_csv(launcher, ech, TS, S, v_lim, v_imp);

    return model, p_lim, p_imposable;
end

function print_nz(variables)
    for x in variables
        if abs(value(x[2])) > 1e-6
            println(x, " = ", value(x[2]));
        end
    end
end

function write_output_csv(launcher::Workflow.Launcher, ech, TS, S, v_lim::Workflow.LimitableModeler, v_imp::Workflow.ImposableModeler)
    open(joinpath(launcher.dirpath, "limitation.csv"), "w") do file
        write(file, @sprintf("%s;%s;%s;%s;%s;%s;\n", "gen", "TS","S", "is_lim", "p_lim", "p0"));
        for x in v_lim.p_enr
            key = x[1];
            gen = key[1];
            ts =  key[2];
            s = key[3];

            is_lim = false;
            if value(v_lim.is_limited[key]) > 0.5
                is_lim = true;
            end
            p_sol = value(v_lim.p_lim[gen, ts]);
            p0 = launcher.uncertainties[gen, s, ts, ech];
            write(file, @sprintf("%s;%s;%s;%s;%f;%f\n", gen, ts,s, is_lim, p_sol, p0));
        end
    end
    open(joinpath(launcher.dirpath, "imposition.csv"), "w") do file
        write(file, @sprintf("%s;%s;%s;%s;%s;%s;\n", "gen", "TS","S", "is_imp", "p_imp", "p0"));
        for x in v_imp.p_imposable
            key = x[1];
            gen, ts, s = key;

            is_imp = false;
            if value(v_imp.p_is_imp[gen, ts]) > 0.5
                is_imp = true;
            end
            p_sol = value(v_imp.p_imposable[gen, ts, s]);
            p0 = launcher.uncertainties[gen, s, ts, ech];
            write(file, @sprintf("%s;%s;%s;%s;%f;%f\n", gen, ts,s, is_imp, p_sol, p0));
        end
    end
end

max_stars_repo_name: azized/pscopf

max_issues_repo_name: azized/pscopf

max_forks_repo_name: azized/pscopf


text:
"""Struct that holds the quantised array as UInts with an additional
field for the min, max of the original range."""
struct LinQuantArray{T,N} <: AbstractArray{Unsigned,N}
    A::Array{T,N}       # array of UInts
    min::Float64        # offset min, max
    max::Float64
end

Base.size(QA::LinQuantArray) = size(QA.A)
Base.getindex(QA::LinQuantArray,i...) = getindex(QA.A,i...)
Base.eltype(Q::LinQuantArray{T,N}) where {T,N} = T

"""Quantise an array linearly into a LinQuantArray."""
function LinQuantization(::Type{T},A::AbstractArray) where {T<:Unsigned}
    all(isfinite.(A)) || throw(DomainError("Linear quantization only in (-∞,∞)"))

    Amin = Float64(minimum(A))              # minimum of value range
    Amax = Float64(maximum(A))              # maximum of value range

    if Amin == Amax
        Δ = 0.0                                 # set to zero for no range
    else
        Δ = (2^(sizeof(T)*8)-1)/(Amax-Amin)     # inverse spacing
    end

    Q = similar(A,T)                        # preallocate

    # map minimum to 0x0, maximum to 0xff...ff
    @inbounds for i in eachindex(Q)
        Q[i] = round((A[i]-Amin)*Δ)
    end

    return LinQuantArray{T,ndims(Q)}(Q,Amin,Amax)
end

# define 8, 16, 24 and 32 bit
LinQuant8Array(A::AbstractArray{T,N}) where {T,N} = LinQuantization(UInt8,A)
LinQuant16Array(A::AbstractArray{T,N}) where {T,N} = LinQuantization(UInt16,A)
LinQuant24Array(A::AbstractArray{T,N}) where {T,N} = LinQuantization(UInt24,A)
LinQuant32Array(A::AbstractArray{T,N}) where {T,N} = LinQuantization(UInt32,A)

"""De-quantise a LinQuantArray into floats."""
function Base.Array{T}(n::Integer,Q::LinQuantArray) where {T<:AbstractFloat}
    Qmin = Q.min                # min as Float64
    Qmax = Q.max                # max as Float64
    Δ = (Qmax-Qmin)/(2^n-1)     # linear spacing

    A = similar(Q,T)

    @inbounds for i in eachindex(A)
        # convert Q[i]::UInt to Float64 via *
        # then to T through =
        A[i] = Qmin + Q[i]*Δ
    end

    return A
end

# define default conversions for 8, 16, 24 and 32 bit
Base.Array{T}(Q::LinQuantArray{UInt8,N}) where {T,N} = Array{T}(8,Q)
Base.Array{T}(Q::LinQuantArray{UInt16,N}) where {T,N} = Array{T}(16,Q)
Base.Array{T}(Q::LinQuantArray{UInt24,N}) where {T,N} = Array{T}(24,Q)
Base.Array{T}(Q::LinQuantArray{UInt32,N}) where {T,N} = Array{T}(32,Q)

Base.Array(Q::LinQuantArray{UInt8,N}) where N = Array{Float32}(8,Q)
Base.Array(Q::LinQuantArray{UInt16,N}) where N = Array{Float32}(16,Q)
Base.Array(Q::LinQuantArray{UInt24,N}) where N = Array{Float32}(24,Q)
Base.Array(Q::LinQuantArray{UInt32,N}) where N = Array{Float64}(32,Q)

# one quantization per layer
"""Linear quantization independently for every element along dimension
dim in array A. Returns a Vector{LinQuantArray}."""
function LinQuantArray(::Type{TUInt},A::AbstractArray{T,N},dim::Int) where {TUInt,T,N}
    @assert dim <= N   "Can't quantize a $N-dimensional array in dim=$dim"
    n = size(A)[dim]
    L = Vector{LinQuantArray}(undef,n)
    t = [if j == dim 1 else Colon() end for j in 1:N]
    for i in 1:n
        t[dim] = i
        L[i] = LinQuantization(TUInt,A[t...])
    end
    return L
end

# for 8,16,24 and 32 bit
LinQuant8Array(A::AbstractArray{T,N},dim::Int) where {T,N} = LinQuantArray(UInt8,A,dim)
LinQuant16Array(A::AbstractArray{T,N},dim::Int) where {T,N} = LinQuantArray(UInt16,A,dim)
LinQuant24Array(A::AbstractArray{T,N},dim::Int) where {T,N} = LinQuantArray(UInt24,A,dim)
LinQuant32Array(A::AbstractArray{T,N},dim::Int) where {T,N} = LinQuantArray(UInt32,A,dim)

"""Undo the linear quantisation independently along one dimension, and returns
an array whereby the dimension always comes last. Hence, might be permuted compared
to the uncompressed array."""
function Base.Array{T}(L::Vector{LinQuantArray}) where T
    N = ndims(L[1])
    n = length(L)
    s = size(L[1])
    t = axes(L[1])
    A = Array{T,N+1}(undef,s...,length(L))
    for i in 1:n
        A[t...,i] = Array{T}(L[i])
    end
    return A
end

ext: jl


text:
# An implementation of the ideas described in A Minimalist's
# Implementation of an Approximate Nearest Neighbor Algorithm
# in Fixed Dimensions.
# Paper: http://cs.uwaterloo.ca/~tmchan/sss.ps
# Further reading: http://en.wikipedia.org/wiki/Z-order_curve

module LowDimNearestNeighbors

export shuffless, shuffmore, shuffeq, preprocess!, nearest

# Return whether the index of the most significant bit
# of m is higher than that of n.
lessmsb(m, n) = m < n && m < (m $ n)

# Find the deciding dimension that determines which
# of p and q comes first in shuffle order. This is
# equivalent to finding the dimension with the most
# significant differing bit between p and q.
# Ties are broken in favor of lower-index dimensions.
function shuffdim(p, q)
	@assert length(p) == length(q)
	@assert length(p) > 0

	# For any integers x and y, the msb of x $ y is
	# the most significant differing bit between
	# x and y.
	k, kxor = 1, p[1] $ q[1]
	for i in 2:length(p)
		ixor = p[i] $ q[i]
		if lessmsb(kxor, ixor)
			k, kxor = i, ixor
		end
	end
	k
end

# Define less-than, more-than, and equality.
shuffless(p, q) = (k = shuffdim(p, q); p[k] < q[k])
shuffmore(p, q) = (k = shuffdim(p, q); p[k] > q[k])
  shuffeq(p, q) = (k = shuffdim(p, q); p[k] == q[k])

# Sort the given array in shuffle order to prepare it for nearest-neighbor queries.
function preprocess!(arr)
	for p in arr
		for i in length(p)
			!(typeof(p[i]) <: Unsigned) && throw(ErrorException("All coordinates must be unsigned integers."))
		end
	end
	sort!(arr, lt=shuffless)
end

# Code for branch-free saturating arithmetic from
# http://locklessinc.com/articles/sat_arithmetic/
satadd{T <: Unsigned}(x::T, y::T) = (res = x + y; res | unsigned(-signed(T(res < x))))
satsub{T <: Unsigned}(x::T, y::T) = (res = x - y; res & unsigned(-signed(T(res <= x))))

function satmul(x::UInt32, y::UInt32)
	res = UInt64(x) * UInt64(y)
	hi::UInt32 = res >> 32
	lo::UInt32 = res & 0x0000ffff
	lo | unsigned(-Int32(hi != 0))
end

function satmul(x::UInt64, y::UInt64)
	res = UInt128(x) * UInt128(y)
	hi::UInt64 = res >> 64
	lo::UInt64 = res & 0x0000000000000000ffffffffffffffff
	lo | unsigned(-Int64(hi != 0))
end

# There's no way for the result to underflow or overflow.
satdiv{T <: Unsigned}(x::T, y::T) = x / y

# Represent shifted points by their own type.
# It is assumed that data is a point with
# eltype(data) <: Unsigned and the value
# of offset fits into the type eltype(data).
immutable ShiftedPos{Q}
	data::Q
	offset::UInt
end
Base.getindex(s::ShiftedPos, args...) = (v = s.data[args...]; v + s.offset > typemax(v) ? typemax(v) : oftype(v, v + s.offset))
# satadd(v, oftype(v, clamp(s.offset, zero(v), typemax(v)))))
Base.length(s::ShiftedPos) = length(s.data)

immutable ShiftedNeg{Q}
	data::Q
	offset::UInt
end
Base.getindex(s::ShiftedNeg, args...) = (v = s.data[args...]; v <s.offset ? zero(v) : oftype(v, v - s.offset))
# satsub(v, oftype(v, clamp(s.offset, zero(v), typemax(v)))))
Base.length(s::ShiftedNeg) = length(s.data)

immutable Result{P, Q}
	point::P
	r_sq::UInt
	bbox_hi::ShiftedPos{Q}
	bbox_lo::ShiftedNeg{Q}
	Result(point::P) = new(point, typemax(UInt))
	function Result(point::P, r_sq, q::Q)
		r = ceil(Integer, sqrt(r_sq))
		new(point, r_sq, ShiftedPos{Q}(q, r), ShiftedNeg{Q}(q, r))
	end
end

# Euclidean distance, though any p-norm will do.
function sqdist(p, q)
	@assert length(p) == length(q)
	@assert length(q) > 0

	d_sq::UInt = 0
	for i in 1:length(p)
		# d_sq += UInt((p[i] - q[i])^2)
		diff = UInt(p[i] < q[i] ? q[i] - p[i] : p[i] - q[i])
		d_sq = satadd(d_sq, satmul(diff, diff))
	end
	d_sq
end

function sqdist_to_quadtree_box(q, p1, p2)
	@assert length(q) == length(p1) == length(p2)
	@assert length(q) > 0

	# Find the most significant differing bit of p1 and p2
	xor = p1[1] $ p2[1]
	for i in 2:length(p1)
		ixor = p1[i] $ p2[i]
		lessmsb(xor, ixor) && (xor = ixor)
	end

	# The power-of-two and size of the quadtree-aligned
	# bounding box that most snugly encloses p1 and p2
	power = xor == 0 ? 1 : 1 + exponent(float(xor))
	size = one(UInt) << power

	# Calculate the squared distance from q to the box.
	# The return value is a float for efficiency;
	# it will be multiplied by another float upon return.
	d_sq = 0.0
	for i in 1:length(q)
		# Compute the coordinates of the bounding box
		bbox_lo = (p1[i] >> power) << power
		bbox_hi = bbox_lo + size

		# Accumulate squared distance from the box
		if q[i] < bbox_lo
			d_sq += (UInt(bbox_lo) - q[i])^2
		elseif q[i] > bbox_hi
			d_sq += (q[i] - bbox_hi)^2
		end
	end
	d_sq
end

function nearest{P, Q, T <: Unsigned}(arr::Array{P}, q::Q, lo::T, hi::T, R::Result{P, Q}, ε::Float64)
	# Return early if the range is empty
	lo > hi && return R

	# Calculate the midpoint of the range, avoiding midpoint overflow
	mid = (lo + hi) >>> 1

	# Compute the distance from the probe point to the query point,
	# and update the result if it's closer than our best match so far
	r_sq = sqdist(arr[mid], q)
	r_sq < R.r_sq && (R = Result{P, Q}(arr[mid], r_sq, q))

	# Return early if the range is only one element wide or if the
	# bounding box containing the range is outside of the search radius
	if lo == hi || sqdist_to_quadtree_box(q, arr[lo], arr[hi]) * (1.0 + ε)^2 >= R.r_sq
		return R
	end

	# Recurse. Unlike binary search, we occasionally recurse into
	# both halves of the array when we can't guarantee that the nearest
	# point lies outside the other half
	if shuffless(q, arr[mid])
		R = nearest(arr, q, lo, mid - 1, R, ε)
		shuffmore(R.bbox_hi, arr[mid]) && (R = nearest(arr, q, mid + 1, hi, R, ε))
	else
		R = nearest(arr, q, mid + 1, hi, R, ε)
		shuffless(R.bbox_lo, arr[mid]) && (R = nearest(arr, q, lo, mid - 1, R, ε))
	end

	R
end

function nearest{P, Q}(arr::Array{P}, q::Q, ε=0.0)
	@assert length(arr) > 0 "Searching for the nearest in an empty array"
	nearest(arr, q, UInt(1), UInt(length(arr)), Result{P, Q}(arr[1]), ε).point
end

end # module


text:
#
# Optimal data formatter for a fixed width screen.
# (see <https://research.google.com/pubs/pub44667.html>)
#

module PrettyPrinting

export
    pprint,
    pprintln

import Base:
    IndexStyle,
    getindex,
    size,
    show,
    *, /, |

const DEFAULT_LINE_WIDTH = 79
const DEFAULT_BREAK_COST = 1
const DEFAULT_SPILL_COST = 2

include("tile.jl")
include("fit.jl")

"""
    pprint([io::IO], data)

Displays the data so that it fits the width of the output screen.
"""
pprint(data) =
    pprint(stdout, data)

pprint(io::IO, data) =
    pprint(io, tile(data))

function pprint(io::IO, lt::Layout)
    render(io, best_fit(io, lt))
    nothing
end

"""
    pprintln([io::IO], data)

Displays the data using `pprint` and adds a newline.
"""
pprintln(data) =
    pprintln(stdout, data)

function pprintln(io::IO, data)
    pprint(io, data)
    println(io)
end

end


text:
@testset "Convex Hull" begin
    A = zeros(50, 30)
    A = convert(Array{Bool}, A)
    A[25, 1] = 1
    A[1, 10] = 1
    A[10, 10] = 1
    A[10, 30] = 1
    A[40, 30] = 1
    A[40, 10] = 1
    A[50, 10] = 1
    B = @inferred convexhull(A)
    C = CartesianIndex{}[]
    push!(C, CartesianIndex{}(25, 1))
    push!(C, CartesianIndex{}(1, 10))
    push!(C, CartesianIndex{}(10, 30))
    push!(C, CartesianIndex{}(40, 30))
    push!(C, CartesianIndex{}(50, 10))
    @test typeof(B) == Array{CartesianIndex{2},1}
    @test sort(B) == sort(C)

    A = [
        0.0,
        0.0,
        1.0,
        0.0,
        0.0,
        0.0,
        1.0,
        1.0,
        0.0,
        0.0,
        1.0,
        0.0,
        0.0,
        1.0,
        1.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        0.0,
        1.0,
        0.0,
        0.0,
    ]
    A = reshape(A, 5, 5)
    A = convert(Array{Bool}, A)
    B = B = @inferred convexhull(A)
    C = CartesianIndex{}[]
    push!(C, CartesianIndex{}(1, 3))
    push!(C, CartesianIndex{}(3, 1))
    push!(C, CartesianIndex{}(3, 5))
    push!(C, CartesianIndex{}(5, 3))
    @test typeof(B) == Array{CartesianIndex{2},1}
    @test sort(B) == sort(C)
end


text:
function rk4_step(r::Vector{Float64}, f::Function, ds::Float64)

    k1 = f(r)
    k2 = f(r + ds*k1/2)
    k3 = f(r + ds*k2/2)
    k4 = f(r + ds*k3)

    return r + ds*(k1 + 2*k2 + 2*k3 + k4) / 6
end

function distance_to_segment(x, y, vert0::Vector{Float64}, vert1::Vector{Float64})

    xs, ys = [x, y] - vert0
    t = begin dv = vert1 - vert0; atan(dv[2], dv[1]) end

    u = xs * cos(t) + ys * sin(t)
    v = -xs * sin(t) + ys * cos(t)

    # the sign function returns 0 at v=0, which kind of messes things up, so have it default to 1 instead.
    sgn = sign(v)
    if sgn == 0
        sgn = 1
    end

    if u < 0
        return norm([x,y] - vert0) * sgn
    elseif 0 <= u < norm(vert1 - vert0)
        return v
    else
        return norm([x,y] - vert1) * sgn
    end
end

function distance_to_arm(x, y, vert0::Vector{Float64}, vert1::Vector{Float64}, vert2::Vector{Float64})

    # find shortest distance
    dist1 = distance_to_segment(x, y, vert0, vert1)
    dist2 = distance_to_segment(x, y, vert1, vert2)
    dist = minimum(abs.([dist1, dist2]))

    # get orientation of the bend
    r1 = vert1 - vert0
    r2 = vert2 - vert1
    bend = sign(r1[1]*r2[2] - r1[2]*r2[1])
    if bend == 0
        bend = 1
    end

    # get correct sign
    sgn = all(bend*[dist1, dist2] .> 0) ? bend : -bend

    return sgn * dist
end

struct Mirror

    verts :: Matrix{Float64}   # all of the vertices
    arms :: Array{Float64, 3}  # vertices in groups of 3

end
Base.getindex(M::Mirror, i) = Mirror(M.verts[:,i], M.arms[:,:,i])

function Mirror(chains::Vector{Matrix{Float64}})

    verts = hcat(chains...)
    arms_list = Array{Float64, 3}[]

    for chain in chains
        N = size(chain)[2]
        a = zeros(2, 3, N)
        a[:,:,1] = hcat(chain[:,end], chain[:,1:2])
        a[:,:,end] = hcat(chain[:,end-1:end], chain[:,1])
        for i=2:size(chain)[2]-1
            a[:,:,i] = chain[:,i-1:i+1]
        end
        append!(arms_list, [a])
    end

    arms = zeros(2, 3, size(verts)[2])
    arms[:,1,:] = hcat([a[:,1,:] for a in arms_list]...)
    arms[:,2,:] = hcat([a[:,2,:] for a in arms_list]...)
    arms[:,3,:] = hcat([a[:,3,:] for a in arms_list]...)

    return Mirror(verts, arms)
end

function distance_to_mirror(x, y, M::Mirror)

    # find nearest arm, return distance to nearest arm
    dists = [distance_to_arm(x, y, M.arms[:,1,k], M.arms[:,2,k], M.arms[:,3,k]) for k=1:size(M.arms)[3]]
    _, kmin = findmin(dists .^ 2)

    return dists[kmin], kmin

    # # get the closest vertex, return distance to arm represented by that vertex
    # _, k = findmin([norm([x,y] - M.verts[:,i]) for i=1:size(M.verts)[2]])
    # vert0 = M.arms[:,1,k]
    # vert1 = M.arms[:,2,k]
    # vert2 = M.arms[:,3,k]
    #
    # return distance_to_arm(x, y, vert0, vert1, vert2), k
end

function mirror_image(x0, y0, dx, dy, M::Mirror)

    H(r) = distance_to_mirror(r[1], r[2], M)[1]
    gradH(r) = ForwardDiff.gradient(H, r)
    f(r) = gradH(r) / norm(gradH(r))
    x = x0
    y = y0
    ds = 0.1*minimum([dx, dy])
    dist = 0
    steps = 0

    while H([x,y]) < 0
        x, y = rk4_step([x,y], f, ds)
        dist += 1
        steps += 2
    end

    for _=1:dist
        x, y = rk4_step([x,y], f, ds)
    end

    i = round((x - x0)/dx)
    j = round((y - y0)/dy)
    while H([x0 + dx*i, y0 + dy*j]) < 0
        x, y = rk4_step([x,y], f, ds)
        i = round((x - x0)/dx)
        j = round((y - y0)/dy)
    end

    i = round((x - x0)/dx)
    j = round((y - y0)/dy)
    x = x0 + dx*i
    y = y0 + dy*j
    return x, y, i, j
end

function smoothstep(x, y, s0, ds, M::Mirror)

    u = (distance_to_mirror(x, y, M)[1] - s0) / ds
    return (u < 0) ? 0.0 : ((0 < u < 1) ? 3*u^2 - 2*u^3 : 1.0)
end

@recipe function f(M::Mirror)

    legend --> false
    seriescolor --> :black
    linewidth --> 3
    aspect_ratio --> :equal

    x = M.arms[1,:,:]
    y = M.arms[2,:,:]
    x, y
end


text:
using LinearAlgebraicRepresentation
Lar = LinearAlgebraicRepresentation
using ViewerGL
GL = ViewerGL

include("./twocubegrids.jl")

# generation of the chain 3-complex of two intersecting cell 2-complexes in 3-space
W, copEV, copFE, copCF = twocubegrids(2,2,2);
# generation of arrays of simplicial complexes: by body, by face, and by edges
V,CVs,FVs,EVs = Lar.pols2tria(W, copEV, copFE, copCF)
# generation and explosion of meshes by body
meshes = GL.GLExplode(V,CVs[1:end],3,3,3,1)
# visualization
GL.VIEW(meshes);


text:
@testset "JS_BWG_HNE" begin
    θ₀ = [0.7, 0.9, 3.3, 5.0, 0.3, 4, 2.7, 0.55, 0.26]
    ω = 1.1
    @testset "Preallocation" begin
        model = JS_BWG_HNE{1}(θ₀)
        @test model.r_over4 == model.r/4
        @test model.ωₚ² == model.ωₚ^2
        @test model.ωₚ³ == model.ωₚ^3
        @test model.ωₚ⁴ == model.ωₚ^4
        @test model.ωₚ⁶ == model.ωₚ^6
        @test model.logγ == log(model.γ)
        @test model.σᵣ_over3 == model.σᵣ/3
        @test model.cosϕₘ == cos(model.ϕₘ)
        @test model.sinϕₘ == sin(model.ϕₘ)
        @test model.cos2ϕₘ == cos(2model.ϕₘ)
        @test model.sin2ϕₘ == sin(2model.ϕₘ)
    end
    @testset "Gradient" begin
        @test approx_gradient(θ -> sdf(JS_BWG_HNE{1}(θ), ω), θ₀)      ≈ grad_sdf(JS_BWG_HNE{1}(θ₀), ω)
        @test approx_gradient(θ -> sdf(JS_BWG_HNE{1}(θ), 0), θ₀)      ≈ grad_sdf(JS_BWG_HNE{1}(θ₀), 0)
    end
    @testset "Hessian" begin
        @test approx_hessian( θ -> grad_sdf(JS_BWG_HNE{1}(θ), ω), θ₀) ≈ hess_sdf(JS_BWG_HNE{1}(θ₀), ω)
        @test approx_hessian( θ -> grad_sdf(JS_BWG_HNE{1}(θ), 0), θ₀) ≈ hess_sdf(JS_BWG_HNE{1}(θ₀), 0)
    end
    @testset "coherancy" begin
        S = sdf(JS_BWG_HNE{1}(θ₀), ω)
        @test coherancy(JS_BWG_HNE{1}(θ₀), ω) ≈ [S[i,j]/sqrt(S[i,i]*S[j,j]) for i in 1:size(S,1), j in 1:size(S,2)]
    end
    @testset "Error handling" begin
        @test_throws ArgumentError JS_BWG_HNE{1}(-0.7,0.9, 3.3, 5.0, 0, 4, 2.7, 0.55, 0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7,-0.9, 3.3, 5.0, 0, 4, 2.7, 0.55, 0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 0.8, 5.0, 0, 4, 2.7, 0.55, 0.26) # 0.8 <1
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 3.3, 0.9, 0, 4, 2.7, 0.55, 0.26) # 0.9 < 1
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 3.3, 5.0, 0,-4, 2.7, 0.55, 0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 3.3, 5.0, 0, 4,-2.7, 0.55, 0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 3.3, 5.0, 0, 4, 2.7,-0.55, 0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(0.7, 0.9, 3.3, 5.0, 0, 4, 2.7, 0.55,-0.26)
        @test_throws ArgumentError JS_BWG_HNE{1}(ones(8))
        @test_throws ArgumentError JS_BWG_HNE{1}(ones(10))
        @test_throws ErrorException JS_BWG_HNE(ones(9))
        @test_throws ErrorException JS_BWG_HNE(0.7, 0.9, 3.3, 5.0, 0, 4, 2.7, 0.55, 0.26)
    end
end


text:
#import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate();
#Write Altro folder instead of @__DIR__
using TrajectoryOptimization
using Plots, LinearAlgebra

#number individual systems
agent_number=10

#defining product system dynamics
function dynamics!(ẋ::AbstractVector{T},x::AbstractVector{T},u::AbstractVector{T}) where T

	g = 9.8;
	M_c=1.0;
	M_p=0.1;
	M_t=M_p+M_c;
	l=0.5;
	c1 = u[1]/M_t;
	c2 = M_p*l/M_t;

	c3=l*4/3;
	c4=l*M_p/M_t;

	F=(g*sin(x[3])-cos(x[3])*(c1+c2*x[4]*x[4]*sin(x[3])))/(c3-c4*cos(x[3])*cos(x[3]));
	G= (c1+c2*x[4]*x[4]*sin(x[3])) - c4* cos(x[3])*F;

	ẋ[1] = x[2];
	ẋ[2] = G;
	ẋ[3]= x[4];
	ẋ[4] = F;

	ẋ[5]=x[6];
	ẋ[6]=u[2];
end

#Total number of states
n=6
#Total number of control inputs
m=2

#defining descrete model
model = Model(dynamics!,n,m)
model_d = rk4(model)
n = model.n;
m = model.m;

T = Float64;

#inital point for product system
x0 = [0.0;0.0;pi;0.0;8.0;0.0]

#goal point for product system
xf = [5.0;0.0;pi;0.0;4.0;0.0]

#number of points in trajectory(number of time samples)
N = 70
#sampling time
dt = 0.1
#total time
tf = (N-1)*dt

#LQR Cost Matrices (same as default)
Q = 0.01*Diagonal(I,n)
Qf = 1000.0*Diagonal(I,n)
R = 0.01*Diagonal(I,m)
obj = LQRObjective(Q,R,Qf,xf,N)

#creating upperbound and lower bound for control input with bound constraint
constraints = Constraints(N)
u_min=[-6.0,-5.0]
u_max=[6.0,5.0]
bnd = BoundConstraint(n,m,u_min=u_min,u_max=u_max)
for k = 1:N-1
     constraints[k] += bnd
 end

#constraint for Collision
#this function is defining penalty value for collision constraint
function My_constraint(x)
	ϵ=0.35;
	l=0.5;
	X1=x[1]+l*sin(x[3]);
	Y1=0.6+l*cos(x[3]);
	X2=x[5]
	Y2=0.2;
	dist=max((X1-X2)^2 , (Y1-Y2)^2)
	return -(dist - ϵ^2)
end

function My_obs(c,x,u)
    c[1]=My_constraint(x);
    c[2]=-1
	return nothing
end

obs= Constraint{Inequality}(My_obs,n,m,2,:obs)
for k = 1:N-1
     constraints[k] += obs
 end

#goal constraint
goal = goal_constraint(xf);
constraints[N] += goal

prob = Problem(model_d,obj,constraints=constraints,x0=x0,N=N,dt=dt,xf=xf)

# options for solver
max_con_viol = 1.0e-8
verbose=false

opts_ilqr = iLQRSolverOptions{T}(verbose=verbose,
    iterations=300)

opts_al = AugmentedLagrangianSolverOptions{T}(verbose=verbose,
    opts_uncon=opts_ilqr,
    iterations=40,
    cost_tolerance=1.0e-5,
    cost_tolerance_intermediate=1.0e-4,
    constraint_tolerance=max_con_viol,
    penalty_scaling=10.,
    penalty_initial=1.)

opts_pn = ProjectedNewtonSolverOptions{T}(verbose=verbose,
    feasibility_tolerance=max_con_viol,
    solve_type=:feasible)

opts_altro = ALTROSolverOptions{T}(verbose=verbose,
    opts_al=opts_al,
    R_inf=1.0e-8,
    resolve_feasible_problem=false,
    opts_pn=opts_pn,
    projected_newton=true,
    projected_newton_tolerance=1.0e-4);

	solver = AugmentedLagrangianSolver(prob)

	#here we call the solver to solve
    @time begin
    	solve!(prob, opts_altro); # solve with ALTRO
    end

	x1 = [prob.X[k][1] for k = 1:N]
	x2 = [prob.X[k][2] for k = 1:N]
	x3 = [prob.X[k][3] for k = 1:N]
	x4 = [prob.X[k][4] for k = 1:N]
	x5 = [prob.X[k][5] for k = 1:N]
	x6 = [prob.X[k][6] for k = 1:N]

	X1=Array{Float64,1}(undef,N)
	X2=x5
	Y1=Array{Float64,1}(undef,N)
	Y2=Array{Float64,1}(undef,N)
	Y3=Array{Float64,1}(undef,N)
	l=0.5
	for i = 1:N
		X1[i]=x1[i]+l*sin(x3[i]);
		Y1[i]=0.6+l*cos(x3[i]);

		Y2[i]=0.2;
		Y3[i]=0.6;

	end

		io = open("nom_tr.txt", "w")
		for i in prob.U
			print(io,i[1])
			print(io," ")
			println(io,i[2])
		end
		close(io)
println("Trajectory is written in nom_tr.txt");

rectangle(w, h, x, y) = Shape(x .+ [0,w,w,0], y .+ [0,0,h,h])

	anim = @animate for i in 1:N
plot((X1[i],Y1[i]),marker=:circle,label="pole position",xlim=(0,7),ylim=(0,7),markersize=5)

plot!((X2[i],Y2[i]),marker=:circle,label="unicycle")

plot!(rectangle(0.54,0.45,X2[i]-0.27,0), opacity=.5)

plot!((x1[i],Y3[i]+0.1),marker=:rect,label="cart position",markersize=8)
plot!([x1[i],X1[i]],[Y3[i],Y1[i]],lw=2,color=:black)

	end

max_stars_repo_name: MehrdadZareian/GMARA

max_issues_repo_name: MehrdadZareian/GMARA

max_forks_repo_name: MehrdadZareian/GMARA


text:
# Power iteration algorithm for computing the spectral norm
function power_iteration!(W, u, n_iterations)
    v = nothing
    for i=1:n_iterations
        Wu = W' * u
        v = Wu ./ (norm(Wu) + eps(Float32))
        Wv = W * v
        u .= Wv ./ (norm(Wv) + eps(Float32))
    end
    u, v
end

# Compute the maximum singular value
msv(u, v, W) = u' * W * v

## Dense Layer with Spectral Normalization
struct DenseSN{F,S<:AbstractMatrix, B, I<:Int, VV<:AbstractArray}
  weight::S
  bias::B
  σ::F
  n_iterations::I # Number of power iterations for computing max singular value
  u::VV # Left vector for power iteration
  function DenseSN(W::M, bias=true, σ::F=identity, n_iterations::I=1, u::VV=randn(Float32, size(W,1), 1)) where {M<:AbstractMatrix, F, I<:Int, VV<:AbstractArray}
    b = Flux.create_bias(W, bias, size(W,1))
    new{F,M,typeof(b), I, VV}(W, b, σ, n_iterations, u)
  end
end

function DenseSN(in::Integer, out::Integer, σ=identity; init=Flux.glorot_uniform, bias=true, n_iterations=1, u=randn(Float32, out, 1))
  DenseSN(init(out, in), bias, σ, n_iterations, u)
end

Flux.@functor DenseSN

Flux.trainable(a::DenseSN) = (a.weight, a.bias)

function (a::DenseSN)(x::AbstractVecOrMat)
  W, b, σ = a.weight, a.bias, a.σ
  u, v = Zygote.ignore(() -> power_iteration!(W, a.u, a.n_iterations))
  σ.((W ./ msv(u, v, W))*x .+ b)
end

(a::DenseSN)(x::AbstractArray) = reshape(a(reshape(x, size(x,1), :)), :, size(x)[2:end]...)

function Base.show(io::IO, l::DenseSN)
  print(io, "DenseSN(", size(l.weight, 2), ", ", size(l.weight, 1))
  l.σ == identity || print(io, ", ", l.σ)
  print(io, ")")
end

## Convluational layer with Spectral Normalization
struct ConvSN{N,M,F,A,V, I<:Int, VV<:AbstractArray}
  σ::F
  weight::A
  bias::V
  stride::NTuple{N,Int}
  pad::NTuple{M,Int}
  dilation::NTuple{N,Int}
  n_iterations::I # Number of power iterations for computing max singular value
  u::VV # Left vector for power iteration
end

function ConvSN(w::AbstractArray{T,N}, b::Union{Flux.Zeros, AbstractVector{T}}, σ = identity;
              stride = 1, pad = 0, dilation = 1, n_iterations = 1) where {T,N}
  stride = Flux.expand(Val(N-2), stride)
  dilation = Flux.expand(Val(N-2), dilation)
  pad = Flux.calc_padding(Conv, pad, size(w)[1:N-2], dilation, stride)
  u = randn(Float32, size(to2D(w), 1), 1)
  return ConvSN(σ, w, b, stride, pad, dilation, n_iterations, u)
end

function ConvSN(k::NTuple{N,Integer}, ch::Pair{<:Integer,<:Integer}, σ = identity;
            init = Flux.glorot_uniform,  stride = 1, pad = 0, dilation = 1,
            weight = Flux.convfilter(k, ch, init = init), bias = Flux.zeros(ch[2]), n_iterations = 1) where N
  ConvSN(weight, bias, σ, stride = stride, pad = pad, dilation = dilation, n_iterations = n_iterations)
end

Flux.@functor ConvSN

Flux.trainable(a::ConvSN) = (a.weight, a.bias)

function (c::ConvSN)(x::AbstractArray)
  σ, b = c.σ, reshape(c.bias, ntuple(_->1, length(c.stride))..., :, 1)
  cdims = DenseConvDims(x, c.weight; stride=c.stride, padding=c.pad, dilation=c.dilation)
  u, v = Zygote.ignore(() -> power_iteration!(to2D(c.weight), c.u, c.n_iterations))
  σ.(conv(x, c.weight ./ msv(u, v, to2D(c.weight)), cdims) .+ b)
end

function Base.show(io::IO, l::ConvSN)
  print(io, "ConvSN(", size(l.weight)[1:ndims(l.weight)-2])
  print(io, ", ", size(l.weight, ndims(l.weight)-1), "=>", size(l.weight, ndims(l.weight)))
  l.σ == identity || print(io, ", ", l.σ)
  print(io, ")")
end


text:
line_regex = r"(\d+)-(\d+) ([a-z]): ([a-z]+)";
lines = readlines("2/input.txt");

function part1(input)
    function check_pw(item)
        return parse(Int64, item[1]) <= count(x -> (x == only(item[3])), item[4]) <= parse(Int64, item[2])
    end

    return count(check_pw.(map(x -> x.captures, match.(line_regex, lines))));
end

function part2(input)
    function check_pw(item)
        return (item[4][parse(Int64, item[1])] == only(item[3])) ⊻ (item[4][parse(Int64, item[2])] == only(item[3]))
    end

    return count(check_pw.(map(x -> x.captures, match.(line_regex, lines))));
end

println("Part 1: $(part1(lines))")
println("Part 2: $(part2(lines))")


text:
function D3Trees.D3Tree(p::POMCPPlanner; title="POMCP Tree", kwargs...)
    warn("""
         D3Tree(planner::POMCPPlanner) is deprecated and may be removed in the future. Instead, please use

             a, info = action_info(planner, b)
             D3Tree(info[:tree])

         Or, you can get this info from a POMDPToolbox History

             info = first(ainfo_hist(hist))
             D3Tree(info[:tree])
         """)
    if isnull(p._tree)
        error("POMCPPlanner has not constructed a tree yet, run `action(planner, belief)` first to construct the tree.")
    end
    return D3Tree(get(p._tree); title=title, kwargs...)
end

function D3Trees.D3Tree(t::POMCPTree; title="POMCP Tree", kwargs...)
    lenb = length(t.total_n)
    lenba = length(t.n)
    len = lenb + lenba
    children = Vector{Vector{Int}}(len)
    text = Vector{String}(len)
    tt = fill("", len)
    link_style = fill("", len)
    style = fill("", len)
    ba_children = [Set{Int}() for i in 1:lenba]
    for (ha_o, c) in t.o_lookup
        ha, o = ha_o
        push!(ba_children[ha], c)
    end
    min_V = minimum(t.v)
    max_V = maximum(t.v)
    for b in 1:lenb
        children[b] = t.children[b] .+ lenb
        text[b] = @sprintf("""
                           o: %s
                           N: %-10d""",
                           b==1 ? "<root>" : node_tag(t.o_labels[b]),
                           t.total_n[b]
                          )
        tt[b] = """
                o: $(b==1 ? "<root>" : node_tag(t.o_labels[b]))
                N: $(t.total_n[b])
                $(length(t.children[b])) children
                """
        link_width = max(1.0, 20.0*sqrt(t.total_n[b]/t.total_n[1]))
        link_style[b] = "stroke-width:$link_width"
    end
    for ba in 1:lenba
        children[ba+lenb] = collect(ba_children[ba])
        text[ba+lenb] = @sprintf("""
                                 a: %s
                                 N: %-7d V: %-10.3g""",
                                 node_tag(t.a_labels[ba]), t.n[ba], t.v[ba])
        tt[ba+lenb] = """
                      a: $(tooltip_tag(t.a_labels[ba]))
                      N: $(t.n[ba])
                      V: $(t.v[ba])
                      $(length(ba_children[ba])) children
                      """
        link_width = max(1.0, 20.0*sqrt(t.n[ba]/t.total_n[1]))
        link_style[ba+lenb] = "stroke-width:$link_width"
        rel_V = (t.v[ba]-min_V)/(max_V-min_V)
        if isnan(rel_V)
            color = colorant"gray"
        else
            color = weighted_color_mean(rel_V, colorant"green", colorant"red")
        end
        style[ba+lenb] = "stroke:#$(hex(color))"
    end
    return D3Tree(children;
                  text=text,
                  tooltip=tt,
                  style=style,
                  link_style=link_style,
                  title=title,
                  kwargs...
                 )

end

Base.show(io::IO, mime::MIME"text/html", t::POMCPTree) = show(io, mime, D3Tree(t))
Base.show(io::IO, mime::MIME"text/plain", t::POMCPTree) = show(io, mime, D3Tree(t))


text:
function to_dataset{N}(wparams::PAWarpParams, imgs::Vector{Array{Float64,N}},
                    shapes::Vector{Shape})
    warped1 = pa_warp(wparams, imgs[1], shapes[1])
    v1 = to_vector(warped1, wparams.warp_map)
    dataset = zeros(Float64, length(v1), length(imgs))
    for i=1:length(imgs)
        warped = pa_warp(wparams, imgs[i], shapes[i])
        dataset[:, i] = to_vector(warped, wparams.warp_map)
    end
    dataset
end

## function to_dataset(wparams::PAWarpParams, imgs::Vector{Array{Float64,3}},
##                     shapes::Vector{Shape})
##     warped1 = pa_warp(wparams, imgs[1], shapes[1])
##     v1 = to_vector(warped1, wparams.warp_map)
##     dataset = zeros(Float64, length(v1), length(imgs))
##     for i=1:length(imgs)
##         warped = pa_warp(wparams, imgs[i], shapes[i])
##         dataset[:, i] = to_vector(warped, wparams.warp_map)
##     end
##     dataset
## end

function to_vector{T}(img::Matrix{T}, mask::Matrix{Int})
    vec = Array(T, 0)
    for j=1:size(img, 2)
        for i=1:size(img, 1)
            if mask[i, j] > 0
                push!(vec, img[i, j])
            end
        end
    end
    vec
end

function to_vector{T}(img::Array{T,3}, mask::Matrix{Int})
    vec = Array(T, 0)
    for k=1:size(img, 3)
        for j=1:size(img, 2)
            for i=1:size(img, 1)
                if mask[i, j] > 0
                    push!(vec, img[i, j])
                end
            end
        end
    end
    vec
end

function to_image{T}(vec::Vector{T}, mask::Matrix{Int})
    img = zeros(T, size(mask))
    k = 1
    for j=1:size(img, 2)
        for i=1:size(img, 1)
            if mask[i, j] > 0
                img[i, j] = vec[k]
                k += 1
            end
        end
    end
    img
end

function load_image_dataset(name::Symbol)
    aam = load_aam()
    imgs = load_images(name, datadir=DATA_DIR_CK,
                       resizeratio=RESIZERATIO)
    shapes = load_shapes(name, datadir=DATA_DIR_CK,
                         resizeratio=RESIZERATIO)
    labels = load_labels

    dataset = to_dataset(aam.wparams, imgs, shapes)
    labels = load_labels(name, datadir=DATA_DIR_CK)
    mask = aam.wparams.warp_map
    return dataset, labels, mask
end

function save_images{T,N}(imgs::Vector{Array{T,N}}, path::UTF8String; prefix="img")
    for i=1:length(imgs)
        img = normalize(imgs[i])
        imwrite(img, joinpath(path, @sprintf("%s_%03d.png", prefix, i)))
    end
end

function save_comps{T}(X::Matrix{T}, mask::Matrix{Int}, path::UTF8String)
    imgs = [to_image(X[:, i], mask) for i=1:size(X, 2)]
    save_images(imgs, path)
end

distance(x, y) = sum(abs(x .- y))

function distance_matrix{T}(X::Matrix{T})
    d, n = size(X)
    D = zeros(Float64, n, n)
    for j=1:n
        for i=1:n
            D[i, j] = distance(X[:, i], X[:, j])
        end
        println("column $j")
    end
    return D
end

function dissimilar(X)
    D = distance_matrix(X)
    R = dbscan(D, 50, 1)
    return X[:, R.seeds]
end

# 1993 - S55
# 1990+1700 - S74

## function train_and_save_aam_ck()
##     imgs_aam = load_images(:ck, datadir=DATA_DIR_CK,
##                            resizeratio=RESIZERATIO, start=2000, count=2000)
##     shapes_aam = load_shapes(:ck, datadir=DATA_DIR_CK,
##                              resizeratio=RESIZERATIO, start=2000, count=2000)
##     @time aam = train(AAModel(), imgs_aam, shapes_aam);
##     save(joinpath(DATA_DIR_CK, "aam.jld"), "aam", aam)
## end

function train_and_save_aam_put()
    imgs = collect(Array{Float64,3},
                       load_images(FaceDatasets.PutFrontalDataset,
                                   joinpath(DATA_DIR, "PUT")));
    shapes = collect(Array{Float64,2},
                         load_shapes(FaceDatasets.PutFrontalDataset,
                                     joinpath(DATA_DIR, "PUT")));
    @time m = train(AAModel(), imgs_aam, shapes_aam)
    save(joinpath(joinpath(DATA_DIR, "models"), "aam_put.jld"), "aam", m)

    for i=1:10
        try
            img_idx = rand(1:length(imgs))
            shape_idx = rand(1:length(imgs))
            triplot(imgs[img_idx], shapes[shape_idx], m.wparams.trigs)
            @time fitted_shape, fitted_app = fit(m, imgs[img_idx], shapes[shape_idx], 30);
            triplot(imgs[img_idx], fitted_shape, m.wparams.trigs)
            println("Image #$img_idx; shape #$shape_idx")
            # readline(STDIN)
        catch e
            if isa(e, BoundsError)
                println("Fitting diverged")
                # readline(STDIN)
            else
            rethrow()
            end
        end
    end
end

function load_aam()
    return load(joinpath(DATA_DIR_CK, "aam.jld"))["aam"]
end

function save_dataset(dataset)
    h5write(joinpath(DATA_DIR_CK, "dataset_10708.h5"), "dataset", dataset)
end

read_dataset() = h5read(joinpath(DATA_DIR_CK, "dataset.h5"), "dataset")

function test_projections()
    P = projection(fit(PCA, dataset))
    nview(to_image(P[:, 1], aam.wparams.warp_map))
    pca_imgs = Matrix{Float64}[normalize(to_image(P[:, i],
                                                  aam.wparams.warp_map))
                               for i=1:size(P, 2)]
    save_images(pca_imgs,
                expanduser("~/Dropbox/PhD/MyPapers/facial_expr_repr/images/pca"))
end

function view_and_save_rbm()
    comps = components(rbm)
    rbm_imgs = [to_image(comps[:, i], mask) for i=1:size(comps, 2)]
    nviewall(rbm_imgs[1:36])

    save_images(convert(Vector{Matrix{Float64}}, rbm_imgs),
                expanduser("~/Dropbox/PhD/MyPapers/facial_expr_repr/images/rbm"))

    h5open(joinpath(DATA_DIR_CK, "rbm_1.h5"), "w") do h5
        save_params(h5, rbm, "rbm")
    end

    h5open(joinpath(DATA_DIR_CK, "rbm_1.h5")) do h5
        load_params(h5, rbm, "rbm")
    end
end

function save_images(dir::AbstractString, imgs::Vector;
                     prefix="img", suffix=".png")
    for (i, img) in enumerate(imgs)
        save(joinpath(dir, prefix * string(i) * suffix), img)
    end
    println("Wrote $(length(imgs)) images to $dir")
end