############################### Continuous device build functions ###########################

function ContinuousBuildContainer()
    return Dict{PSY.PrimeMovers.PrimeMover, Float64}(
        PSY.PrimeMovers.CT => 0.0,
        PSY.PrimeMovers.CC => 0.0,
        PSY.PrimeMovers.ST => 0.0,
        PSY.PrimeMovers.WT => 0.0,
        PSY.PrimeMovers.PVe => 0.0,
        PSY.PrimeMovers.BA => 0.0,
        PSY.PrimeMovers.HY => 0.0,
    )
end

function update_continuous_samples(config, mapping, data)
    for pm in keys(mapping)
        config[pm] = data[mapping[pm]]
    end
    return config
end

function continuous_sampling(
    distribution_dict::Dict{PSY.PrimeMovers.PrimeMover, Distributions.Distribution},
    count::Int,
)
    samples = Dict()
    distribustion_array = []
    distribution_idx = Dict()
    for (ix, (pm, dist)) in enumerate(distribution_dict)
        push!(distribustion_array, dist)
        push!(distribution_idx, pm => ix)
    end
    sample_set = Distributions.Product([distribustion_array...])
    samples_distribution = rand(sample_set, count)
    for ix in 1:count
        _temp = ContinuousBuildContainer()
        _temp =
            update_continuous_samples(_temp, distribution_idx, samples_distribution[:, ix])
        samples[ix] = _temp
    end
    return samples
end

function make_continuous_ct(sys::PSY.System, capacity::Float64)
    devices = []
    buses = PSY.get_components(PSY.Bus, sys)
    capacity = capacity / length(buses)
    for bus in buses
        device = PSY.ThermalStandard(
            "new_CT_$(bus.name)",
            true,
            true,
            bus,
            0.1,
            0.0496,
            0.223606797749979,
            PSY.PrimeMovers.CT,
            PSY.ThermalFuels.OTHER,
            (min = 0.1, max = capacity),
            (min = 0.0, max = 0.1),
            (up = 0.03 * (capacity / 0.3), down = 0.03 * (capacity / 0.3)),
            (up = 1.0, down = 1.0),
            PSY.ThreePartCost(
                PSY.VariableCost([
                    (782.9114112000003, 0.4 * capacity),
                    (1174.3671168, 0.6 * capacity),
                    (1566.6507744000003, 0.8 * capacity),
                    (1995.1987296000002, capacity),
                ],),
                302.86484159999975,
                51.747,
                0.0,
            ),
            0.24,
        )

        push!(devices, device)
    end
    return devices
end

function make_continuous_cc(sys::PSY.System, capacity::Float64)
    devices = []
    buses = PSY.get_components(PSY.Bus, sys)
    capacity = capacity / length(buses)
    for bus in buses
        device = PSY.ThermalStandard(
            "new_CC_$(bus.name)",
            true,
            true,
            bus,
            0.5,
            0.6843,
            3.853894134508627,
            PSY.PrimeMovers.CC,
            PSY.ThermalFuels.NATURAL_GAS,
            (min = 0.5, max = capacity),
            (min = -0.25, max = 1.5),
            (up = 0.0414 * (capacity / 3.55), down = 0.0414 * (capacity / 3.55)),
            (up = 8.0, down = 4.5),
            PSY.ThreePartCost(
                PSY.VariableCost([
                    (3838.085535700569, 0.47 * capacity),
                    (5230.332257410263, 0.65 * capacity),
                    (6941.875221559945, 0.82 * capacity),
                    (8943.709296295801, capacity),
                ],),
                957.5389017269495,
                28046.681022000004,
                0.0,
            ),
            4.14,
        )

        push!(devices, device)
    end
    return devices
end

function make_continuous_st(sys::PSY.System, capacity::Float64)
    devices = []
    buses = PSY.get_components(PSY.Bus, sys)
    capacity = capacity / length(buses)
    for bus in buses
        device = PSY.ThermalStandard(
            "new_ST_$(bus.name)",
            true,
            true,
            bus,
            0.1,
            0.0014000000000000002,
            0.8170679286326199,
            PSY.PrimeMovers.ST,
            PSY.ThermalFuels.NATURAL_GAS,
            (min = 0.1, max = capacity),
            (min = -0.25, max = 0.3),
            (up = 0.02 * (capacity / 0.76), down = 0.02 * (capacity / 0.76)),
            (up = 8.0, down = 4.0),
            PSY.ThreePartCost(
                PSY.VariableCost([
                    (553.9076596522913, 0.39 * capacity),
                    (837.0160191548608, 0.58 * capacity),
                    (1156.6555351904503, 0.79 * capacity),
                    (1501.9025167638265, capacity),
                ],),
                415.84297278910844,
                11172.014351999998,
                0.0,
            ),
            0.89,
        )

        push!(devices, device)
    end
    return devices
end

function make_continuous_wt(sys::PSY.System, capacity::Float64, base_device = "309_WIND_1")
    devices = []
    base_device = PSY.get_components_by_name(PSY.RenewableGen, sys, base_device)[1]
    buses = PSY.get_components(PSY.Bus, sys)
    capacity = capacity / length(buses)
    for bus in buses
        device = deepcopy(base_device)
        IS.clear_forecasts!(device)
        set_device_name!(device, "new_WIND_$(bus.name)")
        set_uuid!(device)
        device.rating = capacity
        PSY.set_bus!(device, bus)
        IS.copy_forecasts!(base_device, device)
        IS.set_time_series_storage!(device, nothing)
        services = PSY.get_services(device)
        while !isempty(services)
            PSY._remove_service!(device, services[1])
        end
        push!(devices, device)
    end
    return devices
end

function make_continuous_pv(sys::PSY.System, capacity::Float64, base_device = "324_PV_3")
    devices = []
    base_device = PSY.get_components_by_name(PSY.RenewableGen, sys, base_device)[1]
    buses = PSY.get_components(PSY.Bus, sys)
    capacity = capacity / length(buses)
    for bus in buses
        device = deepcopy(base_device)
        IS.clear_forecasts!(device)
        set_device_name!(device, "new_PVe_$(bus.name)")
        set_uuid!(device)
        device.rating = capacity
        PSY.set_bus!(device, bus)
        IS.copy_forecasts!(base_device, device)
        IS.set_time_series_storage!(device, nothing)
        services = PSY.get_services(device)
        while !isempty(services)
            PSY._remove_service!(device, services[1])
        end
        push!(devices, device)
    end
    return devices
end

function _make_device(sys::PSY.System, mover::PSY.PrimeMovers.PrimeMover, capacity::Float64)
    if mover == PSY.PrimeMovers.CT
        return make_continuous_ct(sys, capacity)
    elseif mover == PSY.PrimeMovers.CC
        return make_continuous_cc(sys, capacity)
    elseif mover == PSY.PrimeMovers.ST
        return make_continuous_st(sys, capacity)
    elseif mover == PSY.PrimeMovers.WT
        return make_continuous_wt(sys, capacity)
    elseif mover == PSY.PrimeMovers.PVe
        return make_continuous_pv(sys, capacity)
    elseif mover == PSY.PrimeMovers.BA
        return make_continuous_ba(sys, capacity)
    elseif mover == PSY.PrimeMovers.HY
        return make_continuous_hy(sys, capacity)
    end
end

function check_service(service, new_device)
    if (occursin("Reg_Up_R1", service.name)) &&
       (PSY.get_name(PSY.get_bus(new_device))) == "Abel"
        return true
    elseif (occursin("Reg_Up_R2", service.name)) &&
           (PSY.get_name(PSY.get_bus(new_device))) == "Bach"
        return true
    elseif (occursin("Reg_Up_R3", service.name)) &&
           (PSY.get_name(PSY.get_bus(new_device))) == "Cabell"
        return true
    elseif (occursin("Reg_Down_R1", service.name)) &&
           (PSY.get_name(PSY.get_bus(new_device))) == "Abel"
        return true
    elseif (occursin("Reg_Down_R2", service.name)) &&
           (PSY.get_name(PSY.get_bus(new_device))) == "Bach"
        return true
    elseif (occursin("Reg_Down_R3", service.name)) &&
           (PSY.get_name(PSY.get_bus(new_device))) == "Cabell"
        return true
    else
        return false
    end
end


##### fill halo points based on topology
@inline  fill_halo_west!(c, H::Int64, N::Int64, ::Periodic)  = @views @. c[1:H, :, :] = c[N+1:N+H, :, :]
@inline fill_halo_south!(c, H::Int64, N::Int64, ::Periodic)  = @views @. c[:, 1:H, :] = c[:, N+1:N+H, :]
@inline   fill_halo_top!(c, H::Int64, N::Int64, ::Periodic)  = @views @. c[:, :, 1:H] = c[:, :, N+1:N+H]

@inline   fill_halo_east!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[N+H+1:N+2H, :, :] = c[1+H:2H, :, :]
@inline  fill_halo_north!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[:, N+H+1:N+2H, :] = c[:, 1+H:2H, :]
@inline fill_halo_bottom!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[:, :, N+H+1:N+2H] = c[:, :, 1+H:2H]

@inline  fill_halo_west!(c, H::Int64, N::Int64, ::Bounded)   = @views @. c[1:H, :, :] = c[H+1:H+1, :, :]
@inline fill_halo_south!(c, H::Int64, N::Int64, ::Bounded)   = @views @. c[:, 1:H, :] = c[:, H+1:H+1, :]
@inline   fill_halo_top!(c, H::Int64, N::Int64, ::Bounded)   = @views @. c[:, :, 1:H] = c[:, :, H+1:H+1]

@inline   fill_halo_east!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[N+H+1:N+2H, :, :] = c[N+H:N+H, :, :]
@inline  fill_halo_north!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[:, N+H+1:N+2H, :] = c[:, N+H:N+H, :]
@inline fill_halo_bottom!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[:, :, N+H+1:N+2H] = c[:, :, N+H:N+H]

@inline   fill_halo_east_vel!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[N+H+2:N+2H, :, :] = c[N+H+1:N+H+1, :, :]
@inline  fill_halo_north_vel!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[:, N+H+2:N+2H, :] = c[:, N+H+1:N+H+1, :]
@inline fill_halo_bottom_vel!(c, H::Int64, N::Int64, ::Bounded)  = @views @. c[:, :, N+H+2:N+2H] = c[:, :, N+H+1:N+H+1]

@inline   fill_halo_east_Gc!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[N+H+1:N+2H, :, :] = c[1+H:2H, :, :]
@inline  fill_halo_north_Gc!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[:, N+H+1:N+2H, :] = c[:, 1+H:2H, :]
@inline fill_halo_bottom_Gc!(c, H::Int64, N::Int64, ::Periodic) = @views @. c[:, :, N+H+1:N+2H] = c[:, :, 1+H:2H]

@inline   fill_halo_east_Gc!(c, H::Int64, N::Int64, ::Bounded) = @views @. c[N+H+1:N+2H, :, :] = 0.0
@inline  fill_halo_north_Gc!(c, H::Int64, N::Int64, ::Bounded) = @views @. c[:, N+H+1:N+2H, :] = 0.0
@inline fill_halo_bottom_Gc!(c, H::Int64, N::Int64, ::Bounded) = @views @. c[:, :, N+H+1:N+2H] = 0.0

@inline function fill_halo_nut!(nuts::NamedTuple, g::AbstractGrid{TX, TY, TZ}) where {TX, TY, TZ}
    for nut in nuts
          fill_halo_west!(nut.data, g.Hx, g.Nx, TX())
          fill_halo_east!(nut.data, g.Hx, g.Nx, TX())
         fill_halo_south!(nut.data, g.Hy, g.Ny, TY())
         fill_halo_north!(nut.data, g.Hy, g.Ny, TY())
           fill_halo_top!(nut.data, g.Hz, g.Nz, TZ())
        fill_halo_bottom!(nut.data, g.Hz, g.Nz, TZ())
    end
    return nothing
end

@inline function fill_halo_Gcs!(nuts::NamedTuple, g::AbstractGrid{TX, TY, TZ}) where {TX, TY, TZ}
    for nut in nuts
          fill_halo_east_Gc!(nut.data, g.Hx, g.Nx, TX())
         fill_halo_north_Gc!(nut.data, g.Hy, g.Ny, TY())
        fill_halo_bottom_Gc!(nut.data, g.Hz, g.Nz, TZ())
    end
    return nothing
end

@inline function fill_halo_u!(u, g::AbstractGrid{TX, TY, TZ}) where {TX, TY, TZ}
    if isa(TX(), Bounded)
        fill_halo_east_vel!(u, g.Hx, g.Nx, TX())
    else
        fill_halo_east!(u, g.Hx, g.Nx, TX())
    end
      fill_halo_west!(u, g.Hx, g.Nx, TX())
     fill_halo_south!(u, g.Hy, g.Ny, TY())
     fill_halo_north!(u, g.Hy, g.Ny, TY())
       fill_halo_top!(u, g.Hz, g.Nz, TZ())
    fill_halo_bottom!(u, g.Hz, g.Nz, TZ())
end

@inline function fill_halo_v!(v, g::AbstractGrid{TX, TY, TZ}) where {TX, TY, TZ}
    if isa(TY(), Bounded)
        fill_halo_north_vel!(v, g.Hy, g.Ny, TY())
    else
        fill_halo_north!(v, g.Hy, g.Ny, TY())
    end
      fill_halo_west!(v, g.Hx, g.Nx, TX())
      fill_halo_east!(v, g.Hx, g.Nx, TX())
     fill_halo_south!(v, g.Hy, g.Ny, TY())
       fill_halo_top!(v, g.Hz, g.Nz, TZ())
    fill_halo_bottom!(v, g.Hz, g.Nz, TZ())
end

@inline function fill_halo_w!(w, g::AbstractGrid{TX, TY, TZ}) where {TX, TY, TZ}
    if isa(TZ(), Bounded)
        fill_halo_bottom_vel!(w, g.Hz, g.Nz, TZ())
    else
        fill_halo_bottom!(w, g.Hz, g.Nz, TZ())
    end
     fill_halo_west!(w, g.Hx, g.Nx, TX())
     fill_halo_east!(w, g.Hx, g.Nx, TX())
    fill_halo_south!(w, g.Hy, g.Ny, TY())
    fill_halo_north!(w, g.Hy, g.Ny, TY())
      fill_halo_top!(w, g.Hz, g.Nz, TZ())
end

@inline function fill_halo_vel!(vels::NamedTuple, g::AbstractGrid)
    fill_halo_u!(vels.u.data, g)
    fill_halo_v!(vels.v.data, g)
    fill_halo_w!(vels.w.data, g)
    return nothing
end


module TestCaches

using Test
using RigidBodyDynamics
using RigidBodyDynamics.Contact
import Random

function randmech()
    rand_tree_mechanism(Float64,
        QuaternionFloating{Float64},
        [Revolute{Float64} for i = 1 : 5]...,
        [Fixed{Float64} for i = 1 : 5]...,
        [Prismatic{Float64} for i = 1 : 5]...,
        [Planar{Float64} for i = 1 : 5]...,
        [SPQuatFloating{Float64} for i = 1:2]...,
        [SinCosRevolute{Float64} for i = 1:2]...
    )
end

function cachetest(cache, eltypefun)
    x64 = @inferred cache[Float64]
    @test eltypefun(x64) == Float64
    @test cache[Float64] === x64
    @test @allocated(cache[Float64]) == 0

    x32 = @inferred cache[Float32]
    @test eltypefun(x32) == Float32
    @test cache[Float32] === x32
    @test @allocated(cache[Float32]) == 0

    @test cache[Float64] === x64
    @test @allocated(cache[Float64]) == 0
end

@testset "StateCache" begin
    Random.seed!(1)
    mechanism = randmech()
    cache = StateCache(mechanism)
    cachetest(cache, RigidBodyDynamics.state_vector_eltype)
end

@testset "DynamicsResultCache" begin
    @testset "Basic mechanism" begin
        Random.seed!(2)
        mechanism = randmech()
        cache = DynamicsResultCache(mechanism)
        cachetest(cache, result -> eltype(result.v̇))
    end

    @testset "Mechanism with contact points (Issue #483)" begin
        Random.seed!(3)
        mechanism = randmech()
        contactmodel = SoftContactModel(hunt_crossley_hertz(k = 500e3),
            ViscoelasticCoulombModel(0.8, 20e3, 100.))
        body = rand(bodies(mechanism))
        add_contact_point!(body,
            ContactPoint(Point3D(default_frame(body), 0.0, 0.0, 0.0), contactmodel))
        cache = DynamicsResultCache(mechanism)
        cachetest(cache, result -> eltype(result.v̇))
    end
end

@testset "SegmentedVectorCache" begin
    Random.seed!(3)
    mechanism = randmech()
    state = MechanismState(mechanism)
    cache = SegmentedVectorCache(RigidBodyDynamics.ranges(velocity(state)))
    cachetest(cache, eltype)
end

end # module


isdefined(Base, :__precompile__) && __precompile__()

module NLOptControl
# TODO  enable setvalue() functionality

using JuMP
import JuMP: setRHS,
             getvalue,
             setvalue,
             @NLexpression,
             @NLobjective,
             @NLparameter,
             @NLconstraint,
             internalmodel
export @NLexpression,
       @NLobjective,
       @NLparameter,
       @NLconstraint,
       setvalue,
       getvalue,
       setRHS

using Ipopt
using FastGaussQuadrature
using DataFrames
using CSV
using Interpolations
using Parameters
import LinearAlgebra

const pseudoSpectralMethods = [ :lgrExplicit , :lgrImplicit ]
const trapezoidalMethods = [ :trapezoidal, :bkwEuler ]

include("math.jl")

include("types.jl")

include("utils.jl")

# TODO: make a hamiltonian function
export  resultsDir!,
        evalConstraints!,
        postProcess!,
        optimize!,
        interpolateLagrange!,
        interpolateLinear!,
        interpolate_lagrange,
        opt2dfs!,
        defineMPC!,
        initOpt!,
        defineIP!,
        mapNames!,
        simIPlant!,
        updateX0!,
        currentIPState,
        goalReached!,
        simMPC!,
        plant2dfs!,
        predictX0!,
        defineTolerances!,
        linearStateTolerances!,
        newConstraint!,
        evalMaxDualInf,
        states!,
        controls!,
        minDF,
        maxDF,
        savePlantData!,
        saveData,
        linearSpline,
        saveOptData,
        integrate!

include("setup.jl")
export  define,
        defineSolver!,
        configure!,
        dynamics!,
        constraints!,
        NLExpr

include("ps.jl")

include("diffeq.jl")

include("PrettyPlots/PrettyPlots.jl")
using .PrettyPlots

export  minDF,
        maxDF,
        plotSettings,
        _pretty_defaults,
        currentSettings,
        statePlot,
        controlPlot,
        costatesPlot,
        costatesPlots,
        allPlots,
        adjust_axis,
        mpcPlot,
        tPlot,
        optPlot,
        xlims!,
        ylims!,
        plot

end # module


export read_cq_bvecs, read_cq_fvecs, CQ_parameters, dump_CQ_parameters

# Read float binary file produced by CQ, like the codebooks D
function read_cq_fvecs(fname::AbstractString)

  dim, count = zero(Cint), zero(Cint)
  vectors = Matrix{Cfloat}

  open(fname, "r") do fid
    count = read(fid, Cint, 1)[1]
    dim   = read(fid, Cint, 1)[1]
    vectors = read(fid, Cfloat, dim, count)
  end

  return vectors
end

# Read int binary file produced by CQ, like the codes B
function read_cq_bvecs(fname::AbstractString)

  dim, count = zero(Cint), zero(Cint)
  codes = Matrix{Cint}

  open(fname, "r") do fid
    count = read(fid, Cint, 1)[1]
    dim   = read(fid, Cint, 1)[1]
    codes = read(fid, Cint, dim, count)
  end

  return codes
end

# Parameters that CQ receives
@with_kw mutable struct CQ_parameters
  PQ::Bool=false
  NCQ::Bool=false
  CQ::Bool=true
  Search::Bool=true

  # global parameters
  # points_count=1000000
  points_count::Int=100000
  dictionaries_count::Int=8
  words_count::Int=256
  space_dimension::Int=128
  # points_file=/home/julieta/Desktop/local-search-quantization/data/sift/sift_base.fvecs
  points_file::String="/home/julieta/Desktop/local-search-quantization/data/sift/sift_learn.fvecs"
  output_file_prefix::String="/home/julieta/Desktop/CQ/build/temp/"
  max_iter::Int=30

  # PQ parameters
  distortion_tol::Float32=0.0001
  read_partition::Int=0
  partition_file::String=""
  # if 101 then using closure cluster, else lloyd kmeans
  kmeans_method::Int=101

  # NCQ and CQ parameters
  num_sep::Int=20
  # initial from outside, if 1 then set the file name of dictinary and codes
  initial_from_outside=0
  dictionary_file::String=""
  binary_codes_file::String=""

  # CQ parameters
  mu::Float32=0.0004f0

  # Search parameters
  queries_count::Int=10000
  groundtruth_length::Int=100
  result_length::Int=1000
  queries_file::String="/home/julieta/Desktop/local-search-quantization/data/sift/sift_query.fvecs"
  groundtruth_file::String="/home/julieta/Desktop/local-search-quantization/data/sift/sift_groundtruth.ivecs"
  trained_dictionary_file::String="/home/julieta/Desktop/CQ/build/temp/D"
  trained_binary_codes_file::String="/home/julieta/Desktop/CQ/build/temp/B"
  output_retrieved_results_file::String="/home/julieta/Desktop/CQ/build/temp/recall"
end

# Print CQ parameters to a config file that the CQ binary can consume
function dump_CQ_parameters(p::CQ_parameters, fname::String)
  open(fname, "w") do fid
    for (name, typ) in zip(fieldnames(CQ_parameters), CQ_parameters.types)
      if typ == Bool || typ == Int
        println(fid, name, "=", Int(getfield(p, name)))
      else
        println(fid, name, "=", getfield(p, name))
      end
    end
  end
end


#include("common.jl")
#include("algorithm_parameters.jl")
#include("dynamical_systems.jl")

"""
`TMState` is a named tuple of the state variables of a temporal memory.

- `α`: active neurons
- `Π`: predictive neurons
- `WN`: winning neurons
- `Πₛ`: predictive dendritic segments (to calculate winning segments)
- `Mₛ`: matching dendritic segments (didn't receive enough input to activate, but enough to learn)
- `ovp_Mₛ`:
"""
mutable struct TMState
  state::NamedTuple{
    (:α, :Π, :WN, :Πₛ, :Mₛ, :ovp_Mₛ),  # Ncell, Ncell, Ncell, Nseg, Nseg, Nseg
    Tuple{CellActivity, CellActivity, CellActivity,
          BitArray{1}, BitArray{1}, Vector{Int}}
  }
  init_Nseg::Int
end
TMState(Nₙ::Int, Nseg)= TMState((
  α=falses(Nₙ), Π=falses(Nₙ), WN=falses(Nₙ),
  Πₛ=falses(Nseg), Mₛ=falses(Nseg),
  ovp_Mₛ=zeros(Nseg)
), Nseg)
Base.getproperty(s::TMState, name::Symbol)= name === :state ?
    getfield(s,:state) :
    name === :init_Nseg ? getfield(s, :init_Nseg) :
      getproperty(getfield(s,:state),name)
update_TMState!(s::TMState; Nseg,α,Π,WN,Πₛ,Mₛ,ovp_Mₛ)=
    s.state= (α=α, Π= Π, WN= WN, Πₛ= padfalse(Πₛ,Nseg),
              Mₛ= padfalse(Mₛ,Nseg), ovp_Mₛ= padfalse(ovp_Mₛ,Nseg))
reset!(s::TMState)= begin
  Nₙ= length(s.α);
  update_TMState!(s, Nseg=s.init_Nseg, α=falses(Nₙ), Π=falses(Nₙ), WN=falses(Nₙ),
    Πₛ=falses(s.init_Nseg), Mₛ=falses(s.init_Nseg), ovp_Mₛ=zeros(s.init_Nseg))
end

"""
    TemporalMemory(params::TMParams; recurrent= true, distal_input_size=0)

`TemporalMemory` learns to predict sequences of input Sparse Distributed Representations (SDRs),
usually generated by a [`SpatialPooler`](@ref).
It learns to represent each input symbol in the temporal context of the symbols that come before it in the sequence,
using the individual neurons of each minicolumn and their distal synapses.

When considering a neuron layer with proximal and distal synapses, the spatial pooler is a way to
activate and learn the proximal synapses, while the temporal memory is a way to activate and learn
the distal synapses.

Parameters:

- `params`: [`TMParams`](@ref) defining the layer size and algorithm constants
- `recurrent`: presence of recurrent distal connections between the layer's neurons
- `distal_input_size`: the number of presynaptic neurons for each layer with incoming distal connections (apart from same layer's neurons)

## High-order predictions and Ambiguity

The [neurons-thousand-synapses paper](https://www.frontiersin.org/articles/10.3389/fncir.2016.00023/full) describes the
Temporal Memory's properties, and especially the ability to
- make "high-order" predictions, based on previous inputs potentially going far back in time
- represent ambiguity in the form of simultaneous predictions

For more information see figures 2,3 in the paper.

# TM activation

Overview of the temporal memory's process:

1. Activate neurons (fire, proximal synapses)
2. Predict neurons (depolarize, distal/apical synapses)
3. Learn distal/apical synapses:
    - adapt existing synapses
    - create new synapses/dendrites

## Activation

An SDR input activates some minicolumns of the neuron layer.
If some neurons in the minicolum were predicted at the previous step, they activate faster than the rest and inhibit them.
If no neuron was predicted, all the neurons fire (minicolumn bursting).

TODO

---
See also: [`TMParams`](@ref) for parameter and symbol description, [`DistalSynapses`](@ref), [`TMState`](@ref)
"""
struct TemporalMemory
  params::TMParams
  distalSynapses::DistalSynapses
  previous::TMState
  recurrent::Bool
end
function TemporalMemory(params::TMParams; recurrent= true, distal_input_size=0)
  @unpack Nₙ, Nc, k = params
  distal_input_size= Int(distal_input_size)
  @assert recurrent || distal_input_size > 0 "At least 1 of recurrent or external input connections are needed"

  # NOTE: it's not possible to have independent distal synapses for each presynaptic input,
  # because we need a single address space for postsynaptic segments. Otherwise,
  # it would be impossible to mix signals from multiple sources.
  N_presynaptic= recurrent ? Nₙ + distal_input_size : distal_input_size
  Nseg_init= 0

  TemporalMemory(params,
      DistalSynapses(N_presynaptic, Nc, k;
        Nseg_init= Nseg_init, params= DistalSynapseParams(params)),
      TMState(Nₙ,Nseg_init),
      recurrent)
end

Nₙ(tm::TemporalMemory)= tm.params.Nₙ
reset!(tm::TemporalMemory)= begin
  reset!(tm.distalSynapses)
  reset!(tm.previous)
end

"""
`step!(tm::TemporalMemory, c, distal_input=[])` evolves the Temporal Memory to the next timestep
given the minicolumn activations and any distal input.
Returns the state of the region's neurons: active, predictive, bursting (minicolumns)

See also: [`tm_activate`](@ref), [`tm_predict`](@ref)
"""
# Given a column activation pattern `c` (SP output), step the TM
function step!(tm::TemporalMemory, c, distal_input=falses(0))
  s= tm.distalSynapses; p= tm.previous

  α, B, WN= tm_activate(tm, c, p.Π)
  # If recurrent, concatenate distal input activity with this layer's activity
  distal_input= tm.recurrent ? [α;distal_input] : distal_input
  Π, Πₛ, Mₛ, ovp_Mₛ= tm_predict(tm, distal_input)

  # Learn
  WS, WS_burst= calculate_WS!(s, p.Πₛ,p.ovp_Mₛ,α,B)
  # Update winner neurons with entries from bursting columns
  WN[NS(s)*WS_burst .> 0].= true
  step!(s, p.WN,WS, α, p.α,p.Mₛ,p.ovp_Mₛ)
  update_TMState!(p, Nseg=Nₛ(s),
                  α=α, Π=Π, WN=WN, Πₛ=Πₛ, Mₛ=Mₛ, ovp_Mₛ=ovp_Mₛ)
  return (
    active= α,
    predictive= Π,
    bursting= B
  )
end

"""
Active and predictive cells given the minicolumn activations and any distal input.

See also: [`tm_activate`](@ref), [`tm_predict`](@ref)
"""
(tm::TemporalMemory)(c, distal_input=falses(0))= begin
  α= tm_activate(tm,c,tm.previous.Π)[1]
  # If recurrent, concatenate distal input activity with this layer's activity
  distal_input= tm.recurrent ? [α;distal_input] : distal_input
  (
    active= α,
    predictive= tm_predict(tm,distal_input)[1]
  )
end

"""
`tm_activate(tm::TemporalMemory, c, Π)` calculates

1. which minicolumns burst,
2. which neurons in the layer are activated
3. which become predictive

for minicolumn activation `c` (size `Nc`) given by the [`SpatialPooler`](@ref)
and the previously predictive neurons `Π` (size `Nₙ`).

# Returns

1. `a`: neuron activation (`Nₙ`)
2. `B`: bursting minicolumns (`Nc`)
3. `WN`: "winning" neurons (`Nₙ`)

See also: [`tm_predict`](@ref), [`DistalSynapses`](@ref)
"""
function tm_activate(tm::TemporalMemory, c, Π)
  @unpack Nc, k = tm.params

  # bursting minicolumns (Nc)
  burst(c,Π)= c .& .!@percolumn(any, Π, k)
  # minicolumns with a predictive neuron (k × Nc)
  predicted(c,Π)= @percolumn(&,Π,c, k)
  # activation for the whole layer given the predicted/bursting neurons
  activate(A_pred, B)= (A_pred .| B')|> vec
  # cache parts of the output because they're used 2 times
  B= burst(c,Π)           # Nc
  A_pred= predicted(c,Π)  # k × Nc
  return activate(A_pred,B), B, A_pred|>vec
end

"""
`tm_predict(tm::TemporalMemory, α)` calculates which neurons will be predictive at the next step
given the currently active neurons + distal input `α`.
`size(a)` must match the presynaptic length of `tm.distalSynapses`.

# Returns

1. `Π`: predictive neurons (`Nₙ`)
2. `Πₛ`: predictive dendritic segments ('Nₛ') (caching)
3. `Mₛ`: matching dendritic segments (`Nₛ`) (learning)
4. `ovp_Mₛ`: subthreshold-matching dendritic segments (`Nₛ`) (learning)
"""
function tm_predict(tm::TemporalMemory, α::CellActivity)
  @unpack θ_stimulus_activate, θ_stimulus_learn = tm.params
  distal= tm.distalSynapses

  # Segment depolarization (prediction)
  Πₛ= Wd(distal)'α .> θ_stimulus_activate
  # Neuron depolarization (prediction)
  Π(Πₛ)= NS(distal)*Πₛ .> 0  # NOTE: params.θ_segment_act instead of 0
  # Sub-threshold segment stimulation sufficient for learning
  ovp_Mₛ= distal.Dd'α
  Mₛ= ovp_Mₛ .> θ_stimulus_learn
  return Π(Πₛ),Πₛ, Mₛ,ovp_Mₛ
end


path,_ = splitdir(@__FILE__)
import Interpolations
using PyPlot
import JLD
import CSV
import Dierckx

# Packages I played a very minor role
import PreComp
import Composites
import VTKtools

# Packages I played a minor role
import OptParams
import Atmosphere
import CCBlade #new CCBlade package w/o ND splined af data
import BeamFEA
import Snopt
import Gradients

# Packages I played a major role
import BrentMin
import MotorPower
import Akima
# import VLM
include("$path/../oldvlm.jl")
import BPM
import AirfoilPrep

include("$path/../setup.jl") # for types
# rc("figure", figsize=(4.0, 2.5))
rc("figure", figsize=(3.5, 3.2))
rc("font", size=10.0)
rc("lines", linewidth=1.5)
rc("lines", markersize=3.0)
rc("legend", frameon=false)
rc("axes.spines", right=false, top=false)
# rc("figure.subplot", left=0.17, bottom=0.21, top=0.97, right=.65)
rc("figure.subplot", left=0.22, bottom=0.32, top=0.99, right=.95)
rc("axes", color_cycle=["348ABD", "A60628", "009E73", "7A68A6", "D55E00", "CC79A7"])
plot_cycle=["#348ABD", "#A60628", "#009E73", "#7A68A6", "#D55E00", "#CC79A7"]
close("all")

numprops = [2,4,8,16,32]

cr_factor = 100#reverse([50,100, 150])
db_factor = reverse([76,65,63,60])

db_factor_plot = reverse(["<76","65","63","60"])

DETAILED = false

detailed_output_accel = []
detailed_output_climb = []
detailed_output_cruise = []
design_accel = []
design_climb = []
design_cruise = []
detailed_system = []

ranges = zeros(length(numprops),length(db_factor))
CLs_accel = zeros(length(numprops),length(db_factor))
CLs_climb = zeros(length(numprops),length(db_factor))
CLs_cruise = zeros(length(numprops),length(db_factor))

Drags_accel = zeros(length(numprops),length(db_factor))
Drags_climb = zeros(length(numprops),length(db_factor))
Drags_cruise = zeros(length(numprops),length(db_factor))

CDs_accel = zeros(length(numprops),length(db_factor))
CDs_climb = zeros(length(numprops),length(db_factor))
CDs_cruise = zeros(length(numprops),length(db_factor))

total_weight = zeros(length(numprops),length(db_factor))
climb_thrust = zeros(length(numprops),length(db_factor))
gama = zeros(length(numprops),length(db_factor))

ETAs_accel = zeros(length(numprops),length(db_factor))
ETAs_climb = zeros(length(numprops),length(db_factor))
ETAs_cruise = zeros(length(numprops),length(db_factor))

BattMasses_accel = zeros(length(numprops),length(db_factor))
BattMasses_climb = zeros(length(numprops),length(db_factor))
BattMasses_cruise = zeros(length(numprops),length(db_factor))

Power_accel = zeros(length(numprops),length(db_factor))
Power_climb = zeros(length(numprops),length(db_factor))
Power_cruise = zeros(length(numprops),length(db_factor))

DBs_accel = zeros(length(numprops),length(db_factor))
DBs_climb = zeros(length(numprops),length(db_factor))
DBs_cruise = zeros(length(numprops),length(db_factor))

Thrust_accel = zeros(length(numprops),length(db_factor))
Thrust_climb = zeros(length(numprops),length(db_factor))
Thrust_cruise = zeros(length(numprops),length(db_factor))

Velocity_accel = zeros(length(numprops),length(db_factor))
Velocity_climb = zeros(length(numprops),length(db_factor))
Velocity_cruise = zeros(length(numprops),length(db_factor))

Rtips = zeros(length(numprops),length(db_factor))

Propulsion_Mass = zeros(length(numprops),length(db_factor))

takeoff_dist = zeros(length(numprops),length(db_factor))
dist_climb = zeros(length(numprops),length(db_factor))
dist_trans = zeros(length(numprops),length(db_factor))
dist_accel = zeros(length(numprops),length(db_factor))

tip_mach_accel = zeros(length(numprops),length(db_factor))
tip_mach_climb = zeros(length(numprops),length(db_factor))
tip_mach_cruise = zeros(length(numprops),length(db_factor))

aoa_accel = zeros(length(numprops),length(db_factor))
aoa_climb = zeros(length(numprops),length(db_factor))
aoa_cruise = zeros(length(numprops),length(db_factor))

mass_prop = zeros(length(numprops),length(db_factor))
blades_prop = zeros(length(numprops),length(db_factor))

span = zeros(length(numprops),length(db_factor))

mean_chord = zeros(length(numprops),length(db_factor))

for i = 1:length(numprops)
    for j = 1:length(db_factor)
    # try
        filename = "$(path)/p$(numprops[i])_cr$(cr_factor)_db$(db_factor[j])correct_span.jld"
        summary = JLD.load(filename)

        detailed_output_accel = summary["detailed_output_accel"]
        detailed_output_climb = summary["detailed_output_climb"]
        detailed_output_cruise = summary["detailed_output_cruise"]
        design_accel = summary["design_accel"]
        design_climb = summary["design_climb"]
        design_cruise = summary["design_cruise"]
        detailed_system = summary["detailed_system"]

        ranges[i,j] = detailed_system.range_true
        CLs_accel[i,j] = detailed_output_accel.Lift/(0.5*1.225*design_accel.velocity^2*design_accel.w_halfspan*2*design_accel.w_chord_pts[1]) #TODO: rho, Sref
        CLs_climb[i,j] = detailed_output_climb.Lift/(0.5*1.225*design_climb.velocity^2*design_climb.w_halfspan*2*design_climb.w_chord_pts[1]) #TODO: rho, Sref
        CLs_cruise[i,j] = detailed_output_cruise.Lift/(0.5*1.225*design_cruise.velocity^2*design_cruise.w_halfspan*2*design_cruise.w_chord_pts[1]) #TODO: rho, Sref

        CDs_accel[i,j] = detailed_output_accel.Drag/(0.5*1.225*design_accel.velocity^2*design_accel.w_halfspan*2*design_accel.w_chord_pts[1]) #TODO: rho, Sref
        CDs_climb[i,j] = detailed_output_climb.Drag/(0.5*1.225*design_climb.velocity^2*design_climb.w_halfspan*2*design_climb.w_chord_pts[1]) #TODO: rho, Sref
        CDs_cruise[i,j] = detailed_output_cruise.Drag/(0.5*1.225*design_cruise.velocity^2*design_cruise.w_halfspan*2*design_cruise.w_chord_pts[1]) #TODO: rho, Sref

        total_weight[i,j] = detailed_system.total_weight

        climb_thrust[i,j] = detailed_output_climb.total_thrust
        gama[i,j] = detailed_system.gama

        ETAs_accel[i,j] = detailed_output_accel.ETA_total
        ETAs_climb[i,j] = detailed_output_climb.ETA_total
        ETAs_cruise[i,j] = detailed_output_cruise.ETA_total

        BattMasses_accel[i,j] = detailed_system.batt_mass_accel
        BattMasses_climb[i,j] = detailed_system.batt_mass_climb
        BattMasses_cruise[i,j] = detailed_system.batt_mass_cruise

        Rtips[i,j] = design_cruise.p_Rtip

        span[i,j] = design_cruise.w_halfspan*2

        Propulsion_Mass[i,j] = max(detailed_output_accel.propwing_mass,detailed_output_climb.propwing_mass,detailed_output_cruise.propwing_mass)

        Power_accel[i,j] = detailed_output_accel.total_thrust*design_accel.velocity
        Power_climb[i,j] = detailed_output_climb.total_thrust*design_climb.velocity
        Power_cruise[i,j] = detailed_output_cruise.total_thrust*design_cruise.velocity

        Thrust_accel[i,j] = detailed_output_accel.total_thrust
        Thrust_climb[i,j] = detailed_output_climb.total_thrust
        Thrust_cruise[i,j] = detailed_output_cruise.total_thrust

        Drags_accel[i,j] = detailed_output_accel.Drag
        Drags_climb[i,j] = detailed_output_climb.Drag
        Drags_cruise[i,j] = detailed_output_cruise.Drag

        Velocity_accel[i,j] = design_accel.velocity
        Velocity_climb[i,j] = design_climb.velocity
        Velocity_cruise[i,j] = design_cruise.velocity

        DBs_accel[i,j] = maximum(detailed_output_accel.db_test)
        DBs_climb[i,j] = maximum(detailed_output_climb.db_test)
        DBs_cruise[i,j] = maximum(detailed_output_cruise.db_test)

        takeoff_dist[i,j] = detailed_system.dist_climb+detailed_system.dist_accel
        dist_accel[i,j] = detailed_system.dist_accel
        dist_trans[i,j] = detailed_system.xtr
        dist_climb[i,j] = detailed_system.x_left

        tip_mach_accel[i,j] = design_accel.p_Rtip*design_accel.p_rpm/60*2*pi/detailed_output_accel.a
        tip_mach_climb[i,j] = design_climb.p_Rtip*design_climb.p_rpm/60*2*pi/detailed_output_climb.a
        tip_mach_cruise[i,j] = design_cruise.p_Rtip*design_cruise.p_rpm/60*2*pi/detailed_output_cruise.a

        aoa_accel[i,j] = design_accel.w_aoa
        aoa_climb[i,j] = design_climb.w_aoa
        aoa_cruise[i,j] = design_cruise.w_aoa

        mean_chord[i,j] = mean(design_cruise.p_chord_pts)

        mass_prop[i,j] = detailed_output_cruise.mass_prop*design_cruise.numprops*design_cruise.blades #same for all three
        blades_prop[i,j] = design_cruise.blades #same for all three

        if DETAILED
            figname = "cl_p_db$(numprops[i])_cr$(cr_factor)_db$(db_factor[j])"
            figure(figname)
            # maxy = zeros(length(db_factor))
            plot(detailed_output_climb.panels.y/maximum(detailed_output_climb.panels.y),detailed_output_climb.cllocal,".-",label = "local cl")
            plot(detailed_output_climb.panels.y/maximum(detailed_output_climb.panels.y),detailed_output_climb.cl,".-",label = "freestream cl")
            xlabel("wing halfspan")
            ylabel("cl")
            # ylim([0,1.05*ceil(Int,maximum(maxy))])
            # legend(loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
            # savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

        end

    # end
    end
end

# High level results
figname = "Takeoff_Speed_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],takeoff_dist[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(takeoff_dist[:,i])
        plot(numprops,takeoff_dist[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Takeoff Distance (m)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Range_Props_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],ranges[1:end-1,i]/1000.0,"o-",label="$(db_factor_plot[i])")#"1/$(cr_factor[i])")
    else
        maxy[i] = maximum(ranges[:,i]/1000.0)
        plot(numprops,ranges[:,i]/1000.0,"o-",label="$(db_factor_plot[i])")#"1/$(cr_factor[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Range (km)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Accel_Speed_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],dist_accel[1:end-1,i],"o-",label="$(db_factor_plot[i])")#"1/$(cr_factor[i])")
    else
        maxy[i] = maximum(dist_accel[:,i])
        plot(numprops,dist_accel[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Ground Roll Distance (m)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
# legend(title = "Cruise Constraint (m/s)",loc="upper center", ncol = length(cr_factor), bbox_to_anchor=(0.5, 1.30))#loc="center left", bbox_to_anchor=(1, 0.5))
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "span_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],span[1:end-1,i],"o-",label="$(db_factor_plot[i])")#"1/$(cr_factor[i])")
    else
        maxy[i] = maximum(span[:,i])
        plot(numprops,span[:,i],"o-",label="$(db_factor_plot[i])")#"1/$(cr_factor[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Span (m)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "CL_climb_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],CLs_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(CLs_climb[:,i])
        plot(numprops,CLs_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Wing Lift Coefficient (CL)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "CD_cruise_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],CDs_cruise[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(CDs_cruise[:,i])
        plot(numprops,CDs_cruise[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Wing Drag Coefficient (CD)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Noise_climb_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],DBs_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(DBs_climb[:,i])
        plot(numprops,DBs_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Sound Pressure Level (dBa)")
# ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "FlightPathAngle_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],gama[1:end-1,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(gama[:,i]*180/pi)
        plot(numprops,gama[:,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Flight Path Angle (degrees)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Total_Mass_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],total_weight[1:end-1,i]/9.81,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(total_weight[:,i]/9.81)
        plot(numprops,total_weight[:,i]/9.81,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Total Mass (kg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Propulsion_Mass_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Propulsion_Mass[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Propulsion_Mass[:,i])
        plot(numprops,Propulsion_Mass[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propulsion Mass (kg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

Propulsion_Mass

figname = "Rtip_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Rtips[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Rtips[:,i])
        plot(numprops,Rtips[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propeller Radius (m)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Battery_Cruise_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],BattMasses_cruise[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(BattMasses_cruise[:,i])
        plot(numprops,BattMasses_cruise[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Battery Mass (kg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Battery_Total_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],BattMasses_cruise[1:end-1,i]+BattMasses_accel[1:end-1,i]+BattMasses_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(BattMasses_cruise[:,i]+BattMasses_accel[:,i]+BattMasses_climb[:,i])
        plot(numprops,BattMasses_cruise[:,i]+BattMasses_accel[:,i]+BattMasses_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Battery Mass (kg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Propulsion_ETA_Cruise_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],ETAs_cruise[1:end-1,i]*100.0,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(ETAs_cruise[:,i]*100.0)
        plot(numprops,ETAs_cruise[:,i]*100.0,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propulsion Efficiency (%)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Propulsion_ETA_Accel_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],ETAs_accel[1:end-1,i]*100.0,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(ETAs_accel[:,i]*100.0)
        plot(numprops,ETAs_accel[:,i]*100.0,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propulsion Efficiency (%)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Propulsion_ETA_Climb_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],ETAs_climb[1:end-1,i]*100.0,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(ETAs_climb[:,i]*100.0)
        plot(numprops,ETAs_climb[:,i]*100.0,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propulsion Efficiency (%)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Climb_Power_In_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Power_climb[1:end-1,i]./ETAs_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Power_climb[:,i]./ETAs_climb[:,i])
        plot(numprops,Power_climb[:,i]./ETAs_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Climb Power In (watts)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Climb_Power_Out_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Power_climb[1:end-1,i]/1000.0,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Power_climb[:,i]/1000.0)
        plot(numprops,Power_climb[:,i]/1000.0,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Climb Power Out (kW)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Climb_Thrust_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Thrust_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Thrust_climb[:,i])
        plot(numprops,Thrust_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Total Thrust (N)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Climb_Thrust2Weight_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Thrust_climb[1:end-1,i]./(total_weight[1:end-1,i]),"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Thrust_climb[:,i]./(total_weight[:,i]))
        plot(numprops,Thrust_climb[:,i]./(total_weight[:,i]),"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Thrust/Weight")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Climb_NetThrust2Weight_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],(Thrust_climb[1:end-1,i]-Drags_climb[1:end-1,i])./(total_weight[1:end-1,i]),"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum((Thrust_climb[:,i]-Drags_climb[:,i])./(total_weight[:,i]))
        plot(numprops,(Thrust_climb[:,i]-Drags_climb[:,i])./(total_weight[:,i]),"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Net-Thrust/Weight")
ylim([0,0.9])
# legend(title = "Cruise Constraint (m/s)",loc="upper center", ncol = length(cr_factor), bbox_to_anchor=(0.5, 1.30))#loc="center left", bbox_to_anchor=(1, 0.5))
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Takeoff_Energy_Percent_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    # if i = || i==2= 1 #Don't plot 16 props for tight db constraint, very bad
    #     to_percent = 100[1:end-1].0*1(end-1BattMasses_accel[:,i]+BattMasses_climb[:,i])./(BattMasses_accel[:,i]+BattMasses_climb[:,i]+BattMasses_cruise[:,i])
    # else
        to_percent = 100.0*(BattMasses_accel[:,i]+BattMasses_climb[:,i])./(BattMasses_accel[:,i]+BattMasses_climb[:,i]+BattMasses_cruise[:,i])
    # end
    semilogy(numprops,to_percent,"o-",label="$(db_factor_plot[i])")
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Takeoff Energy (%)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Velocity_cruise_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Velocity_cruise[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Velocity_cruise[:,i])
        plot(numprops,Velocity_cruise[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Cruise Velocity (m/s)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Velocity_cruiseMPH_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Velocity_cruise[1:end-1,i]*2.237,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Velocity_cruise[:,i]*2.237)
        plot(numprops,Velocity_cruise[:,i]*2.237,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Cruise Velocity (mph)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "tip_mach_climb_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],tip_mach_climb[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(tip_mach_climb[:,i])
        plot(numprops,tip_mach_climb[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Tip Speed (mach)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "aoa_cruise_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],aoa_cruise[1:end-1,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(aoa_cruise[:,i]*180/pi)
        plot(numprops,aoa_cruise[:,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Wing Angle of Attack (deg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "aoa_accel_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],aoa_accel[1:end-1,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(aoa_accel[:,i]*180/pi)
        plot(numprops,aoa_accel[:,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Wing Angle of Attack (deg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "aoa_climb_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],aoa_climb[1:end-1,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(aoa_climb[:,i]*180/pi)
        plot(numprops,aoa_climb[:,i]*180/pi,"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Wing Angle of Attack (deg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "prop_mean_chord_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],mean_chord[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(mean_chord[:,i])
        plot(numprops,mean_chord[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Mean Propeller Chord (m)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "mass_prop_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],mass_prop[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(mass_prop[:,i])
        plot(numprops,mass_prop[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propeller Mass (kg)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "blades_prop_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],blades_prop[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(blades_prop[:,i])
        plot(numprops,blades_prop[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Propeller Blades (#)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Velocity_accel_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=1:length(db_factor)
    if false #i == 1 || i==2 #Don't plot 16 props for tight db constraint, very bad
        plot(numprops[1:end-1],Velocity_accel[1:end-1,i],"o-",label="$(db_factor_plot[i])")
    else
        maxy[i] = maximum(Velocity_accel[:,i])
        plot(numprops,Velocity_accel[:,i],"o-",label="$(db_factor_plot[i])")
    end
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Liftoff Speed (m/s)")
ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Relative_Distance_cr100_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=2#1:length(db_factor)

    plot(numprops,dist_accel[:,i],"o-",label="Accel")

    plot(numprops,dist_trans[:,i],"o-",label="Transition")

    plot(numprops,dist_climb[:,i],"o-",label="Climb")
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Distance (m)")
# ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "Percent_Relative_Distance_db65_db"
figure(figname)
maxy = zeros(length(db_factor))
for i=2#1:length(db_factor)

    plot(numprops,dist_accel[:,i]./takeoff_dist[:,i]*100,"o-",label="Accel")

    plot(numprops,dist_trans[:,i]./takeoff_dist[:,i]*100,"o-",label="Transition")

    plot(numprops,dist_climb[:,i]./takeoff_dist[:,i]*100,"o-",label="Climb")
end
xticks(numprops)
xlabel("Propellers (#)")
ylabel("Relative Distance (%)")
# ylim([0,1.05*ceil(Int,maximum(maxy))])
legend(loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "distance_noise_8"
figure(figname)
maxy = zeros(length(db_factor))
distance_noise_8 = zeros(length(db_factor))
for i=1:length(db_factor)
    distance_noise_8[i] = takeoff_dist[3,i]
end
db_factor[end] = round(Int,DBs_climb[3,4])
plot(db_factor,distance_noise_8,"ko-")
xticks(db_factor)
yticks([0.0;round(distance_noise_8[[1,2,4]],-1)])
xlabel("Sound Pressure Level (dBa)")
ylabel("Takeoff Distance (m)")
ylim([0,1.05*ceil(Int,1.1*maximum(distance_noise_8))])
# legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)

figname = "distance_noise_16"
figure(figname)
maxy = zeros(length(db_factor))
distance_noise_16 = zeros(length(db_factor))
for i=1:length(db_factor)
    distance_noise_16[i] = takeoff_dist[4,i]
end
db_factor[end] = round(Int,DBs_climb[4,4])
plot(db_factor,distance_noise_16,"ko-")
xticks(db_factor)
yticks([0.0;round(distance_noise_16[[1,2,4]],-1)])
xlabel("Sound Pressure Level (dBa)")
ylabel("Takeoff Distance (m)")
ylim([0,1.05*ceil(Int,1.1*maximum(distance_noise_16))])
# legend(title = "Noise Constraint (dBa)",loc="upper center", ncol = length(db_factor), bbox_to_anchor=(0.4, -.20))#loc="center left", bbox_to_anchor=(1, 0.5))
# savefig("$path/../../MDAO_Paper/figures/$figname.pdf",transparent = true)


function Base.promote_shape(a::Tuple{Vararg{Union{AbstractUnitRange,AbstractIndex}}}, b)
    return _promote_shape(a, b)
end
function Base.promote_shape(a, b::Tuple{Vararg{Union{AbstractUnitRange,AbstractIndex}}})
    return _promote_shape(a, b)
end
function Base.promote_shape(
    a::Tuple{Vararg{Union{AbstractUnitRange,AbstractIndex}}},
    b::Tuple{Vararg{Union{AbstractUnitRange,AbstractIndex}}}
   )
    return _promote_shape(a, b)
end

function _promote_shape(a, b)
    if length(a) < length(b)
        return promote_shape(b, a)
    end
    for i=1:length(b)
        if a[i] != b[i]
            throw(DimensionMismatch("dimensions must match"))
        end
    end
    for i=length(b)+1:length(a)
        if a[i] != 1:1
            throw(DimensionMismatch("dimensions must match"))
        end
    end
    return a
end


# This file is part of RLinearAlgebra.jl

module ProceduralTestLinSysSolveRoutine

using Test, RLinearAlgebra

@testset "Linear System Solver Routine Abstractions" begin

    # Verify parent types
    @test supertype(LinSysVecRowProjection) == LinSysSolveRoutine
    @test supertype(LinSysVecColProjection) == LinSysSolveRoutine
    @test supertype(LinSysBlkRowProjection) == LinSysSolveRoutine
    @test supertype(LinSysBlkColProjection) == LinSysSolveRoutine
    @test supertype(LinSysPreconKrylov) == LinSysSolveRoutine

end

end # End module


export print_merge_logs, print_clique_sizes

"""
    AbstractMergeStrategy

An abstract supertype for merge strategies.

A merge strategy determines how the cliques in a clique tree / clique graph are merged to improve computation time of the projection. Each merge strategy should
implement the following functions:

 - initialise!: Initialise the graph / tree from the input clique graph / tree, allocate memory
 - traverse: A method that determines how the clique tree / graph is traversed
 - evaluate: A method that decides whether to merge two cliques
 - update!: A method to update local strategy-related information after a merge
"""
abstract type AbstractMergeStrategy end

"""
    AbstractTreeBasedMerge

An abstract supertype for merge strategies that work with the clique tree.
"""
abstract type AbstractTreeBasedMerge <: AbstractMergeStrategy end

"""
    AbstractGraphBasedMerge

An abstract supertype for merge strategies that work with the clique (intersection) graph.
"""
abstract type AbstractGraphBasedMerge <: AbstractMergeStrategy end

"""
		MergeLog

A struct to analyse the clique merges. Introduced for debugging purposes.
"""
mutable struct MergeLog
	num::Int64 # number of merges
	clique_pairs::Array{Int64, 2} # ordered pair merges
	decisions::Array{Bool, 1} # at what step was merged
	function MergeLog()
		new(0, zeros(0, 2), Array{Bool}(undef, 0))
	end
end

"""
	SuperNodeTree

A structure to represent and analyse the sparsity pattern of the input matrix L.

### Note:
Based on the `merge_strategy` in the constructor, SuperNodeTree might be initialised
as a graph, i.e. seperators `sep` are left empty as well as `snd_child` and `snd_post`.

After cliques have been merged, a valid clique tree will be recomputed from the
consolidated clique graph.
"""
mutable struct SuperNodeTree
	snd::Union{Array{Set{Int64}, 1}, Array{Array{Int64, 1}, 1}} #vertices of supernodes stored in one array (also called residuals)
	snd_par::Array{Int64,1}  # parent of supernode k is supernode j=snd_par[k]
	snd_post::Array{Int64,1} # post order of supernodal elimination tree
	snd_child::Array{Set{Int64},1}
	post::Array{Int64} # post ordering of the vertices in elim tree σ(j) = v
	par::Array{Int64}
	sep::Union{Array{Set{Int64},1}, Array{Array{Int64, 1}, 1}} #vertices of clique seperators
	nBlk::Array{Int64,1} #sizes of submatrizes defined by each clique, sorted by post-ordering, e.g. size of clique with order 3 => nBlk[3]
	num::Int64 # number of supernodes / cliques in tree
	merge_log::MergeLog
	strategy::AbstractMergeStrategy
	function SuperNodeTree(L::SparseMatrixCSC,  merge_strategy::AbstractMergeStrategy)
		par = etree(L)
		child = child_from_par(par)
		post = post_order(par, child)
		# sorting of children from highest order one to lowest make sure that a node always gets
		# added to the supernode with the highest rep vertex
		#sort_children!(child, post)
		# faster algorithm according to Vandenberghe p.308
		degrees = higher_degrees(L)
		snd, snd_par = find_supernodes(par, post, degrees, merge_strategy)

		snd_child = child_from_par(snd_par)
	 	snd_post = post_order(snd_par, snd_child)

		# If the merge strategy is clique graph-based, we give up the tree structure and add the seperators to the supernodes
		# the supernodes then represent the full clique
		# after the clique merging a new clique tree will be computed before psd completion is performed
	 	if typeof(merge_strategy) <: AbstractGraphBasedMerge
			sep = [Set{Int64}() for i = 1:length(snd)]
			add_separators!(L, snd, sep, snd_par, post)
			@. snd_par = -1
			snd_child = [Set{Int64}() for i = 1:length(snd)]
			new(snd, snd_par, snd_post, snd_child, post, par, sep, [0], length(snd_post), MergeLog(), merge_strategy)
	 	# If the merge strategy is tree based keep the supernodes and separators in to different locations
		else
	 		# given the supernodes (clique residuals) find the clique separators
			sep = find_separators(L, snd, snd_par, post, merge_strategy)
			new(snd, snd_par, snd_post, snd_child, post, par, sep, [0], length(snd_post), MergeLog(), merge_strategy)

		end
	end

	function COSMO.SuperNodeTree(cliques::Array{Set{Int64},1}, N::Int64)
	    snd_child = [Set{Int64}() for i = 1:N]
	    snd_par = -1 * ones(Int64, length(cliques))
	    sep = [Set{Int64}() for i = 1:length(cliques)]
	    snd_post = zeros(Int64, length(cliques))
	    new(cliques, snd_par, snd_post, snd_child, collect(1:N), [0], sep, [1], length(cliques), COSMO.MergeLog(), COSMO.CliqueGraphMerge())
	end

	# FIXME: only for debugging purposes
	function SuperNodeTree(snd, par, snd_post, sep, merge_strategy; post::Array{Int64, 1} = [1])
		child = child_from_par(par)
  	new(snd, par, snd_post, child, post, [1], sep, [1], length(snd), MergeLog(), merge_strategy)
	end
end

function sort_children!(child, post)
	for v = 1:length(child)
		child[v] = sort(child[v], by = x -> post[x], rev = true)
	end
end
# -------------------------------------
# FUNCTION DEFINITIONS
# -------------------------------------
# given v=σ^-1(i) it returns i=σ(v)
function invert_order(sigma::Array{Int64,1})
	sigma_inv = zeros(Int64, length(sigma))
	for iii=1:length(sigma)
		sigma_inv[sigma[iii]] = iii
	end
	return sigma_inv
end

# elimination tree algorithm from H.Liu - A Compact Row Storage Scheme for Cholesky Factors Using Elimination Trees
# function etree_liu(g)
# 	N = length(g.adjacency_list)
# 	par = zeros(Int64,N)
# 	ancestor = zeros(Int64,N)

# 	elemSequence = g.reverse_ordering[collect(1:N)]
# 	for iii in elemSequence
# 		for vk in g.adjacency_list[iii]
# 			if g.ordering[vk] < g.ordering[iii]
# 				r = vk
# 				while (ancestor[r] != 0) && (ancestor[r] != iii)
# 					t = ancestor[r]
# 					ancestor[r] = iii
# 					r = t
# 				end
# 				if ancestor[r] == 0
# 					ancestor[r] = iii
# 					par[r] = iii
# 				end
# 			end
# 		end
# 	end

# 	return par
# end

# simplified version of my own elimination tree algorithm with simplified data structure (fastest)
function etree(L)
	N = size(L, 1)
	par = zeros(Int64, N)
	# loop over Vertices of graph
	for i=1:N
		par_ = find_parent_direct(L, i)
		par[i] = par_
	end
	return par
end

# perform a depth-first-search to determine the post order of the tree defined by parent and children vectors
# FIXME: This can be made a lot faster for the case that merges happened, i.e. Nc != length(par)

function post_order(par::Array{Int64,1}, child::Union{Array{Array{Int64, 1}, 1}, Array{Set{Int64}, 1}}, Nc::Int64)

	order = (Nc + 1) * ones(Int64, length(par))
	root = findall(x ->x == 0, par)[1]
	stack = Array{Int64}(undef, 0)
	iii = Nc
	push!(stack, root)
	while !isempty(stack)
		v = pop!(stack)
		order[v] = iii
		iii -= 1
		push!(stack, child[v]...)
	end
	post = collect(1:length(par))
	sort!(post, by = x-> order[x])

	# if merges happened, remove the entries pointing to empty arrays / cliques
	Nc != length(par) && resize!(post, Nc)
	return post
end
post_order(par::Array{Int64,1}, child::Union{Array{Array{Int64, 1}, 1}, Array{Set{Int64},1}}) = post_order(par, child, length(par))

function child_from_par(par::Array{Int64,1})

	child = [Set{Int64}() for i = 1:length(par)]
	for i = 1:length(par)
		par_ = par[i]
		par_ != 0 && push!(child[par_], i)
	end
	return child
end

# ------------------------------------------
# Interfaces function to SuperNodeTree
# ------------------------------------------

function num_cliques(sntree::SuperNodeTree)
	return sntree.num::Int64
end

function get_post_order(sntree::SuperNodeTree)
	return sntree.snd_post
end

function get_post_order(sntree::SuperNodeTree, i::Int64)
	return sntree.snd_post[i]
end
# Using the post order ensures that no empty arrays from the clique merging are returned
function get_snd(sntree::SuperNodeTree, ind::Int64)
		return sntree.snd[sntree.snd_post[ind]]
end

function get_sep(sntree::SuperNodeTree, ind::Int64)
		return sntree.sep[sntree.snd_post[ind]]
end

function get_clique_par(sntree::SuperNodeTree, clique_ind::Int64)
		return sntree.snd_par[sntree.snd_post[clique_ind]]
end
# the block sizes are stored in post order, e.g. if clique 4 (stored in pos 4) has order 2, then nBlk[2] represents the cardinality of clique 4
function get_nBlk(sntree::SuperNodeTree, ind::Int64)
		return sntree.nBlk[ind]::Int64
end

function get_overlap(sntree::SuperNodeTree, ind::Int64)
		return length(sntree.sep[sntree.snd_post[ind]])
end

"Returns the number of rows and the number of overlaps of all the blocks (cliques) represented in the tree after decomposition."
function get_decomposed_dim(sntree::SuperNodeTree, C::DecomposableCones{<: Real})
	dim = 0
	overlaps = 0
	for iii = 1:num_cliques(sntree)
		dim += vec_dim(get_nBlk(sntree, iii), C)
		overlaps += vec_dim(get_overlap(sntree, iii), C)
	end
	return dim::Int64, overlaps::Int64
end

"Given the side dimension of a PSD cone return the number of stored entries."
vec_dim(side_dim::Int64, C::PsdCone{<:Real}) = Base.power_by_squaring(side_dim, 2)
vec_dim(side_dim::Int64, C::PsdConeTriangle{<:Real}) = div(side_dim * (side_dim + 1), 2)

" Return clique with post order `ind` (prevents returning empty arrays due to clique merging)"
get_clique(sntree::SuperNodeTree, ind::Int64) = get_clique(sntree, ind, sntree.strategy)
function get_clique(sntree::SuperNodeTree, ind::Int64, strategy::AbstractTreeBasedMerge)
	c = sntree.snd_post[ind]
	return union(sntree.snd[c], sntree.sep[c])
end

function get_clique(sntree::SuperNodeTree, ind::Int64, strategy::AbstractGraphBasedMerge)
	# if a clique tree has been recomputed, call the method for AbstractTreeBasedMerge types
	if strategy.clique_tree_recomputed
		return get_clique(sntree, ind, COSMO.NoMerge())
	else
		c = sntree.snd_post[ind]
		return sntree.snd[c]
	end
end

function get_clique_by_ind(sntree::SuperNodeTree, ind::Int64)
	return union(sntree.snd[ind], sntree.sep[ind])
end

function print_cliques(sp; reordered = true)
	sntree = sp.sntree
	ordering = sp.ordering
	Nsnd = length(sntree.snd)
	println("Cliques of Graph:")
	println("Reordered = $(reordered)")
	for iii = 1:Nsnd
			if !reordered
				snd = map(x-> ordering[x], sntree.snd[iii])
				sep = map(x-> ordering[x], sntree.sep[iii])
			else
				snd = sntree.snd[iii]
				sep = sntree.sep[iii]
			end
			println("$(iii): res: $(snd), sep: $(sep)")
	end
end
print_cliques(sntree::SuperNodeTree) = print_cliques(sntree, sntree.strategy)
function print_cliques(sntree::SuperNodeTree, strategy::AbstractTreeBasedMerge)
	Nsnd = length(sntree.snd)
	println("Cliques of Graph:")
	for iii = 1:Nsnd
			println("$(iii): res: $(sntree.snd[iii]), sep: $(sntree.sep[iii])")
	end
end

function print_clique_orders(sntree::SuperNodeTree, strategy::AbstractTreeBasedMerge)
	Nsnd = length(sntree.snd)
	println("Cliques of Graph:")
	for iii = 1:Nsnd
		post = sntree.post
		postInv = invert_order(post) #σ-1(v) = j

  	snd_ip = map(x -> postInv[x], sntree.snd[iii])
  	sep_ip = map(x -> postInv[x], sntree.sep[iii])
		println("$(iii): res: $(snd_ip), sep: $(sep_ip)")
	end
end

function print_cliques(sntree::SuperNodeTree, strategy::AbstractGraphBasedMerge)

	if strategy.clique_tree_recomputed
		return print_cliques(sntree, COSMO.NoMerge())
	else
		Nsnd = length(sntree.snd)
		println("Cliques of Graph:")
		for iii = 1:Nsnd
				println("$(iii): clique: $(sntree.snd[iii])")
		end
	end
	return nothing
end

function print_clique_sizes(ws)
  sp_arr = ws.ci.sp_arr
  # print the merge logs for each sparsity pattern
  println(">>> Clique Dimensions:")
  for (iii, sp) in enumerate(sp_arr)
    println("Sparsity Pattern Nr. $(iii), Graph Size: $(length(sp.sntree.par))")

    t = sp.sntree
    Nc = length(t.snd_post)
    # block sizes
    sizes = zeros(Int64, Nc)
    # occurences of block size
    occ = zeros(Int64, Nc)
    for jjj = 1:Nc
      c = t.snd_post[jjj]
      dim = length(t.snd[c]) + length(t.sep[c])
      # try to check if that dimension occured before, if not add, otherwise increment occ
      ind = findfirst(x -> x == dim, sizes)

      if ind != nothing
        occ[ind] = occ[ind] + 1
      else
        sizes[jjj] = dim
        occ[jjj] = 1
      end
    end

    # consolidate
    filter!(x -> x != 0, occ)
    filter!(x -> x != 0, sizes)

    # sort sizes
    p = sortperm(sizes)
    sizes = sizes[p]
    occ = occ[p]

    for (jjj, dim) in enumerate(sizes)
      println("$(occ[jjj])x dim:$(dim)")
    end
  end
end

function check_degree_condition(v::Int64, w::Int64, degrees::Array{Int64,1})
	return degrees[v] > degrees[w] - 1
end

# Algorithm from A. Poten and C. Sun: Compact Clique Tree Data Structures in Sparse Matrix Factorizations (1989)
function pothen_sun(par::Array{Int64,1}, post::Array{Int64,1}, degrees::Array{Int64,1})
	N = length(par)
	sn_ind = -1 * ones(Int64, N) # if snInd[v] < 0 then v is a rep vertex, otherwise v ∈ supernode[snInd[v]]
	supernode_par = -1 * ones(Int64, N)
	children = 	[Array{Int64}(undef, 0) for i = 1:length(par)]

	root_ind = findfirst(x -> x == 0, par)
	# go through parents of vertices in post_order
	for v in post

		if par[v] == 0
			push!(children[root_ind], v)
		else
			push!(children[par[v]], v)
		end

		# parent is not the root
		if par[v] != 0
			if degrees[v] - 1 == degrees[par[v]] && sn_ind[par[v]] == -1
				# Case A: v is a representative vertex
				if sn_ind[v] < 0
					sn_ind[par[v]] = v
					sn_ind[v] -= 1
				# Case B: v is not representative vertex, add to sn_ind[v] instead
				else
					sn_ind[par[v]] = sn_ind[v]
					sn_ind[sn_ind[v]] -= 1
				end
			else
				if sn_ind[v] < 0
					supernode_par[v] = v
				else
					supernode_par[sn_ind[v]] = sn_ind[v]
				end
			end
		end

		# k: rep vertex of the snd that v belongs to
		if sn_ind[v] < 0
			k = v
		else
			k = sn_ind[v]
		end
		# loop over v's children
		v_children = children[v]
		if !isempty(v_children)
			for w in v_children
				if sn_ind[w] < 0
					l = w
				else
					l = sn_ind[w]
				end
				if l != k
					supernode_par[l] = k
				end
			end
		end
	end # loop over vertices

	# representative vertices
	reprv = findall(x-> x < 0, sn_ind)
	# vertices that are the parent of representative vertices
	repr_par = supernode_par[reprv]
	# take into account that all non-representative arrays are removed from the parent structure
	sn_par = zeros(Int64, length(reprv))

	for (iii, rp) in enumerate(repr_par)
		ind = findfirst(x -> x == rp, reprv)
		ind == nothing && (ind = 0)
		sn_par[iii] = ind
	end

	return sn_par, sn_ind
end

function initialise_sets(N::Int64, strategy::AbstractGraphBasedMerge)
	return [Set{Int64}() for i = 1:N]
end

function initialise_sets(N::Int64, strategy::AbstractMergeStrategy)
	return [Int64[] for i = 1:N]
end

function find_supernodes(par::Array{Int64,1}, post::Array{Int64,1}, degrees::Array{Int64,1}, strategy::AbstractMergeStrategy)
	supernode_par, snInd = pothen_sun(par, post, degrees)
	# number of vertices
	N = length(par)
	# number of representative vertices == number of supernodes
	Nrep = length(supernode_par)
	# snode = [Set{Int64}() for i = 1:N]
	snode = initialise_sets(N, strategy)
	for iii = 1:N
		f = snInd[iii]
		if f < 0
			push!(snode[iii], iii)
		else
			push!(snode[f], iii)
		end
	end
	filter!(x -> !isempty(x), snode)
	return snode, supernode_par

end

function find_separators(L, snodes::Union{Array{Set{Int64}, 1}, Array{Array{Int64, 1},1}}, supernode_par::Array{Int64,1}, post::Array{Int64,1}, strategy::AbstractMergeStrategy)
	postInv = invert_order(post)

	Nc = length(supernode_par)
	sep = initialise_sets(Nc, strategy)

	for iii = 1:Nc
		vRep = snodes[iii][1]

		adjPlus = find_higher_order_neighbors(L, vRep)
		deg = length(adjPlus) + 1
		sep[iii] = adjPlus
		setdiff!(sep[iii], snodes[iii])

	end

	return sep

end

function add_separators!(L::SparseMatrixCSC, snodes::Array{Set{Int64},1}, separators::Array{Set{Int64}, 1}, supernode_par::Array{Int64,1}, post::Array{Int64,1})
	postInv = invert_order(post)

	Nc = length(supernode_par)

	for iii = 1:Nc
		snode = snodes[iii]
		sep = separators[iii]
		vRep = minimum(snode)

		adjPlus = find_higher_order_neighbors(L, vRep)
		for neighbor in adjPlus
			if !in(neighbor, snode)
				push!(snode, neighbor)
				push!(sep, neighbor)
			end
		end
	end
	return nothing
end

"""
		reorder_snd_consecutively!(sntree, ordering)

Takes a SuperNodeTree and reorders the vertices in each supernode (and separator) to have consecutive order.

The reordering is needed to achieve equal column structure for the psd completion of the dual variable `Y`. This also modifies `ordering` which maps the vertices in the `sntree` back to the actual location in the not reordered data, i.e.
the primal constraint variable `S` and dual variables `Y`.
"""
function reorder_snd_consecutively!(t::SuperNodeTree, ordering::Array{Int64, 1})

	# determine permutation vector p and permute the vertices in each snd
	p = zeros(Int64,length(t.post))
	snd = t.snd
	sep = t.sep

	k = 1
	for snd_ind in t.snd_post
	  s = snd[snd_ind]
	  l = length(s)
	  p[k:k+l-1] = sort(collect(s))
	  s[:] = collect(k:1:k+l-1)
	  k += l
	end

  # permute the separators as well
  p_inv = invperm(p)
  for sp in t.sep
    sp[:] = map(x -> p_inv[x], sp)
  end

	# # permute the degrees, supernodal parent structure and supernodal post order

	# snd_post_inv = invperm(t.snd_post)
	# root_ind = findfirst(x -> x == 0, t.snd_par)
	# t.snd_par[root_ind] = root_ind
	# t.snd_par = sndpost_inv[t.snd_par[t.snd_post]]
	# t.snd_post = collect(1:1:t.num)

	permute!(ordering, p)
	return nothing
end

function find_higher_order_neighbors(L::SparseMatrixCSC, v::Int64)
	v == size(L, 1) && return 0
	col_ptr = L.colptr
	row_val = L.rowval
	return row_val[col_ptr[v]:col_ptr[v + 1] - 1]
end

function find_parent_direct(L::SparseMatrixCSC, v::Int64)
	v == size(L, 1) && return 0
	col_ptr = L.colptr
	row_val = L.rowval
	return row_val[col_ptr[v]]
end

# findall the cardinality of adj+(v) for all v in V
function higher_degrees(L::SparseMatrixCSC)
	N = size(L, 1)
	degrees = zeros(Int64, N)
	col_ptr = L.colptr
	row_val = L.rowval

	for v = 1:N-1
		degrees[v] = col_ptr[v + 1] - col_ptr[v]
	end
	return degrees
end

# -------------------------------------
# FUNCTION DEFINITIONS
# -------------------------------------

# this assumes a sparse lower triangular matrix L
function connect_graph!(L::SparseMatrixCSC)
	# unconnected blocks don't have any entries below the diagonal in their right-most column
	m = size(L, 1)
	row_val = L.rowval
	col_ptr = L.colptr
	for j = 1:m-1
		connected = false
		for k in col_ptr[j]:col_ptr[j+1]-1
			if row_val[k] > j
				connected  = true
				break
			end
		end
		if !connected
			L[j+1, j] = 1
		end
	end
end

"""
	find_graph!(ci, rows::Array{Int64, 1}, N::Int64, C::AbstractConvexSet)

Given the indices of non-zero rows in `rows`:
- Compute the sparsity pattern and find a chordal extension using `QDLDL` with AMD ordering `F.perm`.
- If unconnected, connect the graph represented by the cholesky factor `L`
"""
function find_graph!(ci, rows::Array{Int64, 1}, N::Int64, C::AbstractConvexSet)
	row_val, col_val = COSMO.row_ind_to_matrix_indices(rows, N, C)
	F = QDLDL.qdldl(sparse(row_val, col_val, ones(length(row_val)), N, N), logical = true)#, perm = collect(1:N))
	nz_ind_map = get_nz_ind_map(rows, N)
	# this takes care of the case that QDLDL returns an unconnected adjacency matrix L
	connect_graph!(F.L)
	ci.L = F.L
	return F.perm, nz_ind_map
end

" A sparse vector that maps the index of svec(i, j) = k to the actual index of where that entry is stored in `A.rowval`."
function get_nz_ind_map(rows::Array{Int64, 1}, N::Int64)
	d = div(N * (N+1), 2)
	nzind = rows
	nzval = collect(1:length(rows))

	return SparseArrays._sparsevector!(nzind, nzval, d)
end

# given an array [rows] that represent the nonzero entries of a vectorized NxN matrix,
# return the rows and columns of the nonzero entries of the original matrix
function row_ind_to_matrix_indices(rows::Array{Int64,1}, N::Int64, ::PsdCone{Float64})
	row_val = zeros(Int64, length(rows))
	col_val = zeros(Int64, length(rows))
	for (ind, r) in enumerate(rows)
		_rem = mod(r, N)
		fl = fld(r, N)
		if _rem == 0
			row_val[ind] = N
			col_val[ind] = fl
		else
			row_val[ind] = _rem
			col_val[ind] = fl + 1
		end
	end
	return row_val, col_val
end

# given an array [rows] that represent the nonzero entries of the vectorized upper triangular part of a NxN matrix,
# return the rows and columns of the nonzero entries of the original matrix
function row_ind_to_matrix_indices(rows::Array{Int64,1}, N::Int64, ::COSMO.PsdConeTriangle{Float64})
	#  allocate conservative here since we don't know how many diagonal entries are contained in row
	row_val = zeros(Int64, 2 * length(rows) + N)
	col_val = zeros(Int64, 2 * length(rows) + N)
	ind = 1
	_step = 1
	for (iii, r) in enumerate(rows)
		i, j = COSMO.svec_to_mat(r)

		row_val[ind] = i
		col_val[ind] = j
		ind += 1

		if i != j
			row_val[ind] = j
			col_val[ind] = i
			ind += 1
		end
	end
	return row_val[1:ind - 1], col_val[1:ind - 1]
end

# Given a linear index find the col of the corresponding upper triangular matrix
function svec_get_col(x::Int64)
	c = (sqrt(8 * x + 1) - 1) / 2
	if c % 1 != 0
		return Int((floor(c) + 1))
	else
		return Int(c)
	end
end

# Given a linear index find the row of the corresponding upper triangular matrix
function svec_get_row(x::Int64)
	c = get_col(x) - 1
	k = (c + 1) * c / 2
	return (x - k)::Int64
end

# Given a linear index find the row and column of the corresponding upper triangular matrix
function svec_to_mat(ind::Int64)
	c = svec_get_col(ind)
	k = (c - 1) * c / 2
	r = Int(ind - k)
	return r::Int64, c::Int64
end