module PurlinDesigner

#Import local modules.
using ..AISIS10016
using ..AISIS10024
using ..ThinWalledBeam
using ..InternalForces
using ..Mesh
using ..BeamColumn
using ..CrossSection

# export line_strength, free_flange_define
export define_purlin_cross_section

function define_purlin_cross_section(cross_section_type, t, d1, b1, h, b2, d2, α1, α2, α3, α4, α5, r1, r2, r3, r4, n, n_radius)

    if cross_section_type == "Z"

        #Define the top Z purlin surface.   For the top flange, this means the out-to-out dimensions.  For the bottom flange, the interior outside dimensions should be used.

        #First calculate the correction on out-to-out length to go from the outside surface to the inside bottom flange surface.
        delta_lip_bottom = t / tan((π - deg2rad(abs(α2 - α1))) / 2)
        delta_web_bottom = t / tan((π - deg2rad(abs(α3 - α2))) / 2)

        #Note here that the bottom flange and lip dimensions are smaller here.
        ΔL = [d1 - delta_lip_bottom, b1 - delta_lip_bottom - delta_web_bottom, h - delta_web_bottom, b2, d2]
        θ = [α1, α2, α3, α4, α5]

        #Note that the outside radius is used at the top flange, and the inside radius is used for the bottom flange.
        radius = [r1 - t, r2 - t, r3, r4]

        closed_or_open = 1

        purlin = CrossSection.Feature(ΔL, θ, n, radius, n_radius, closed_or_open)

        #Calculate the out-to-out purlin surface coordinates.
        xcoords_out, ycoords_out = CrossSection.get_xy_coordinates(purlin)

        #Calculate centerline purlin coordinates.
        unitnormals = CrossSection.surface_normals(xcoords_out, ycoords_out, closed_or_open)
        nodenormals = CrossSection.avg_node_normals(unitnormals, closed_or_open)
        xcoords_center, ycoords_center = CrossSection.xycoords_along_normal(xcoords_out, ycoords_out, nodenormals, -t/2)

        #Shift y coordinates so that the bottom purlin face is at y = 0.
        ycoords_center = ycoords_center .- minimum(ycoords_center) .+ t/2

        #Shift x coordinates so that the purlin web centerline is at x = 0.
        index = floor(Int, length(xcoords_center)/2)
        xcoords_center = xcoords_center .- xcoords_center[index]

    end

    #Add C section here at some point.

    #Package nodal geometry.
    node_geometry = [xcoords_center ycoords_center]

    #Define cross-section element connectivity and thicknesses.
    num_cross_section_nodes = length(xcoords_center)
    element_info = [1:(num_cross_section_nodes - 1) 2:num_cross_section_nodes ones(num_cross_section_nodes - 1) * t]

    return node_geometry, element_info

end

# struct PurlinLineProperties

#     Fy::Array{Float64, 1}
#     E::Array{Float64, 1}
#     μ::Array{Float64, 1}
#     G::Array{Float64, 1}
#     ho::Array{Float64, 1}
#     h::Array{Float64, 1}
#     t::Array{Float64, 1}
#     b_top::Array{Float64, 1}
#     b_bottom::Array{Float64, 1}
#     d::Array{Float64, 1}
#     θc::Array{Float64, 1}
#     Ixx::Array{Float64, 1}
#     Iyy::Array{Float64, 1}
#     c_y_top::Array{Float64, 1}
#     c_y_bottom::Array{Float64, 1}
#     c_x_left::Array{Float64, 1}
#     c_x_right::Array{Float64, 1}
#     Cw::Array{Float64, 1}
#     Wn::Array{Float64, 1}
#     Iyy_flange::Array{Float64, 1}
#     c_x_flange_left::Array{Float64, 1}
#     c_x_flange_right::Array{Float64, 1}
#     Sxx_top::Array{Float64, 1}
#     Sxx_bottom::Array{Float64, 1}
#     Syy_left::Array{Float64, 1}
#     Syy_right::Array{Float64, 1}
#     Syy_flange_left::Array{Float64, 1}
#     Syy_flange_right::Array{Float64, 1}
#     My_xx_top::Array{Float64, 1}
#     My_xx_bottom::Array{Float64, 1}
#     My_xx::Array{Float64, 1}
#     My_yy_left::Array{Float64, 1}
#     My_yy_right::Array{Float64, 1}
#     My_yy::Array{Float64, 1}
#     My_yy_flange_left::Array{Float64, 1}
#     My_yy_flange_right::Array{Float64, 1}
#     My_yy_flange::Array{Float64, 1}
#     Mcrℓ_xx::Array{Float64, 1}
#     Mcrℓ_yy::Array{Float64, 1}
#     Bcrℓ::Array{Float64, 1}
#     Mcrℓ_yy_flange::Array{Float64, 1}
#     CorZ::Array{Int, 1}
#     f1::Array{Float64, 1}
#     f2::Array{Float64, 1}
#     Lm::Array{Float64, 1}
#     M1::Array{Float64, 1}
#     M2::Array{Float64, 1}
#     kϕ_dist::Array{Float64, 1}
#     Sf_dist::Array{Float64, 1}
#     Mcrd::Array{Float64, 1}
#     a::Array{Float64, 1}
#     kv::Array{Float64, 1}
#     Fcrv::Array{Float64, 1}
#     Mne_xx::Array{Float64, 1}
#     Mne_yy::Array{Float64, 1}
#     Mne_flange_yy::Array{Float64, 1}

# end

# function define(member_definitions, dm, dz, material_properties, section_properties, cross_section_dimensions)

#     #MATERIAL PROPERTIES

#     #Define purlin yield stress.
#     Fy = Mesh.create_line_element_property_array(member_definitions, dm, dz, material_properties, 4, 3)

#     #Define purlin steel elastic modulus.
#     E = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, material_properties, 4, 1)

#     #Define purlin steel Poisson's ratio.
#     μ = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, material_properties, 4, 2)

#     #Define purlin steel shear modulus.
#     G= E./(2 .*(1 .+ μ))

#     #CROSS-SECTION DIMENSIONS

#     #Define out to out purlin web height.
#     ho = Mesh.create_line_element_property_array(member_definitions, dm, dz, cross_section_dimensions, 7, 2)

#     #Define flat purlin web width.
#     h = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 7, 7)

#     #Define purlin base metal thickness.
#     t = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, cross_section_dimensions, 7, 1)

#     #Define purlin out-to-out top flange width.
#     b_top = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, cross_section_dimensions, 3, 3)

#     #Define purlin out-to-out bottom flange width.
#     b_bottom = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, cross_section_dimensions, 3, 3)

#     #Define purlin out-to-out flange lip length.
#     d = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, cross_section_dimensions, 3, 4)

#     #Define purlin lip angle from the horizontal, in degrees.
#     θc = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, cross_section_dimensions, 3, 5)

#     #SECTION PROPERTIES

#     #Define purlin centroidal x-x moment of inertia.
#     Ixx = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 1)

#     #Define purlin centroidal y-y moment of inertia.
#     Iyy = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 2)

#     #Define y distance from purlin centroid to the top fiber.
#     c_y_top =  Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 1)  #check this...

#     #Define y distance from purlin centroid to the bottom fiber.
#     c_y_bottom =  Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 1)  #check this...

#     #Define x distance from purlin centroid to the leftmost fiber.
#     c_x_left =  Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 1)  #check this...

#     #Define x distance from purlin centroid to the rightmost fiber.
#     c_x_right =  Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 1)  #check this...

#     #Define purlin warping torsion constant.
#     Cw = Mesh.create_line_element_property_array(member_definitions, dm, dz, sectionProperties, 3, 5)

#     #Define normalized maximum warping stress in the purlin cross-section.
#     Wn = Mesh.create_line_element_property_array(member_definitions, dm, dz, sectionProperties, 3, 6)

#     #Define moment of inertia of the bottom purlin flange.
#     Iyy_flange = Mesh.create_line_element_property_array(member_definitions, dm, dz, FlangeProperties, 3, 2)

#     #Define x distance from purlin bottom flange centroid to the leftmost fiber.
#     c_x_flange_left = Mesh.create_line_element_property_array(member_definitions, dm, dz, FlangeProperties, 3, 2) #check this...

#     #Define x distance from purlin bottom flange centroid to the rightmost fiber.
#     c_x_flange_right = Mesh.create_line_element_property_array(member_definitions, dm, dz, FlangeProperties, 3, 2) #check this...

#     #Calculate the purlin section modulus for the top fiber about the x-x ais.
#     Sxx_top = Ixx ./ c_y_top

#     #Calculate the purlin section modulus for the bottom fiber about the x-x axis.
#     Sxx_bottom = Ixx ./ c_y_bottom

#     #Calculate the purlin section modulus for the leftmost fiber about the y-y ais.
#     Syy_left = Iyy ./ c_y_left

#     #Calculate the purlin section modulus for the rightmost fiber about the y-y axis.
#     Syy_right = Iyy ./ c_y_right

#     #Calculate the bottom purlin flange section modulus for the leftmost fiber about the y-y ais.
#     Syy_flange_left = Iyy_flange ./ c_x_flange_left

#     #Calculate the bottom purlin flange section modulus for the rightmost fiber about the y-y axis.
#     Syy_flange_right = Iyy_flange ./ c_x_flange_right

#     #FIRST YIELD MOMENTS

#     #Define purlin first yield moment about x-x centroidal axis.
#     My_xx_top = Fy .* Sxx_top
#     My_xx_bottom = Fy .* Sxx_bottom
#     My_xx = min.([My_xx_top  My_xx_bottom])

#     #Define purlin first yield moment about y-y centroidal axis.
#     My_yy_left = Fy .* Syy_left
#     My_yy_right = Fy .* Syy_right
#     My_yy = min.([My_yy_left  My_yy_right])

#     #Define purlin bottom flange first yield moment about y-y centroidal axis.
#     My_yy_flange_left = Fy .* Syy_flange_left
#     My_yy_flange_right = Fy .* Syy_flange_right
#     My_yy_flange = min.([My_yy_flange_left  My_yy_flange_right])

#     #CRITICAL ELASTIC LOCAL BUCKLING

#     #Define the purlin critical elastic local buckling moment about the centroidal x-x axis.
#     Mcrℓ_xx = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 7)

#     #Define the purlin critical elastic local buckling moment about the centroidal y-y axis.
#     Mcrℓ_yy = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 8)

#     #Define the purlin critical elastic local buckling moment from torsion.
#     Bcrℓ = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 9)

#     #Define the purlin bottom flange elastic local buckling moment about the centroidal y-y axis.
#     Mcrℓ_yy_flange = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 9)  #check this!!!

#     #CRITICAL ELASTIC DISTORTIONAL BUCKLING

#     #Define the cross-section type, Cee or Zee.
#     CorZ = trunc.(Int, Mesh.create_line_element_property_array(member_definitions, dm, dz, crossSectionDimensions, 3, 6))

#     #Define the stress gradient at the top of the web for bending about the x-x cetroidal axis.
#     f1 = Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 9)  #check this!!!

#     #Define the stress gradient at the bottom of the web for bending about the x-x cetroidal axis.
#     f2= Mesh.create_line_element_property_array(member_definitions, dm, dz, section_properties, 3, 9)  #check this!!!

#     #Define the spacing between discrete distortional buckling restraints.
#     Lm = Mesh.create_line_element_property_array(member_definitions, dm, dz, bracingProperties, 6, 3)

#     #Define moment gradient along the purlin line.  Use this to calculate Mcrd.
#     M1=1.0*ones(length(numberOfNodes))  #need to figure this out...
#     M2=1.0*ones(length(numberOfNodes))  #need to figure this out...

#     #Define the rotational stiffness provided by sheathing and connections to the purlin top flange.  Use this to calculate Mcrd.
#     kϕ_dist=0.0*zeros(length(numberOfNodes))  #need to figure this out...

#     #Define the section modulus at the extreme compression fiber.  Use this to calculate Mcrd.
#     Sf_dist = Sxx   #need to figure this out...

#     #Calculate Mcrd along the purlin line.
#     Mcrd=AISIS10016.app23331.(CorZ, t, ho, b, d, θc, E, μ, G, f1, f2, M1, M2, curvatureSign, Lm, kϕ, Sf)

#     #CRITICAL ELASTIC SHEAR BUCKLING STRESS

#     #Define the shear stiffener spacing along the purlin line.
#     a = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, bracingProperties, 6, 4)

#     #Calculate the plate buckling coefficient for pure shear.
#     kv  = AISIS10016.g233.(a, h)

#     #Caclulate the critical elastic shear buckling stress for the purlin web.
#     Fcrv = AISIS10016.g232.(E, μ, kv, h, t)

#     #GLOBAL BUCKLING NOMINAL FLEXURAL STRENGTH

#     #Define purlin global buckling nominal flexural strength about the x-x centroidal axis. Set this equal to the first yield moment because global buckling deformations will be taken into account in the second order analysis.
#     Mne_xx = My_xx

#     #Define purlin global buckling nominal flexural strength about the y-y centroidal axis.
#     Mne_yy = My_yy

#     #Define purlin bottom flange global buckling nominal flexural strength about the y-y centroidal axis.
#     Mne_flange_yy = My_flange_yy

#     purlin_line = PurlinLineProperties(Fy, E, μ, G, ho, h, t, b_top, b_bottom, d, θc, Ixx, Iyy, c_y_top, c_y_bottom, c_x_left, c_x_right, Cw, Wn, Iyy_flange, c_x_flange_left, c_x_flange_right, Sxx_top, Sxx_bottom, Syy_left, Syy_right, Syy_flange_left, Syy_flange_right, My_xx_top, My_xx_bottom, My_xx, My_yy_left, My_yy_right, My_yy, My_yy_flange_left, My_yy_flange_right, My_yy_flange, Mcrℓ_xx, Mcrℓ_yy, Bcrℓ, Mcrℓ_yy_flange, CorZ, f1, f2, Lm, M1, M2, kϕ_dist, Sf_dist, Mcrd, a, kv, Fcrv, Mne_xx, Mne_yy, Mne_flange_yy)

#     return purlin_line

# end

# #Calculate local-global buckling interaction strength along the purlin line.
# function purlin_local_global_buckling_strength(Mne, Mcrℓ, ASDorLRFD)

#     num_nodes = length(Mne)

#     Mnℓ = zeros(Float64, num_nodes)
#     eMnℓ = zeros(Float64, num_nodes)

#     for i in eachindex(Mcrℓxx)
#         Mnℓ[i], eMnℓ[i] =  AISIS10016.f321(Mne[i], Mcrℓ[i], ASDorLRFD)
#     end

#     return Mnℓ, eMnℓ

# end

# #Calculate torsion strength along the purlin line.
# function purlin_torsion_strength(Cw, Fy, Wn, Bcrℓ, ASDorLRFD)

#     num_nodes = length(Cw)

#     Bn = zeros(Float64, numberOfNodes)
#     eBn = zeros(Float64, numberOfNodes)

#     for i in eachindex(Bcrℓ)
#         Bn[i], eBn[i] = AISIS10024.h411(Cw[i], Fy[i], Wn[i], Bcrℓ[i], ASDorLRFD)
#     end

#     return Bn, eBn

# end

# #Calculate distortional buckling strength along the purlin line.
# function purlin_distortional_buckling_strength(My, Mcrd, ASDorLRFD)

#     num_nodes = length(My)

#     Mnd = zeros(Float64, numberOfNodes)
#     eMnd = zeros(Float64, numberOfNodes)

#     for i in eachindex(Mcrd)
#         Mnd[i], eMnd[i] = AISIS10016.f411(My[i], Mcrd[i], ASDorLRFD)
#     end

#     return Mnd, eMnd

# end

# #Calculate shear strength along the purlin line.
# function purlin_shear_strength(E, h, t, Fy, Vcr, ASDorLRFD)

#     Vn = zeros(Float64, numberOfNodes)
#     eVn = zeros(Float64, numberOfNodes)

#     for i in eachindex(Vcr)
#     Vn[i], eVn[i] = AISIS10016.g21(E[i], h[i], t[i], Fy[i], Vcr[i], ASDorLRFD)
#     end

#     return Vn, eVn

# end

# #Calculate the free flange strength along the purlin line.
# function purlin_free_flange_strength(My_yy, Mcrℓ_yyf, ASDorLRFD)

#     Mnℓyy_freeflange = zeros(Float64, numberOfNodes)
#     eMnℓyy_freeflange = zeros(Float64, numberOfNodes)
#     for i in eachindex(Mnℓyy_freeflange)
#         Mnℓyy_freeflange[i], eMnℓyy_freeflange[i] = AISIS10016.f321(My_yyf[i], Mcrℓ_yyf, ASDorLRFD)
#     end

# end

# #Calculated section strengths along a purlin line.
# function section_strengths(ASDorLRFD, dz, dm, member_definitions, section_properties, cross_section_dimensions, material_properties, load_location, bracing_properties, roof_slope, flange_properties)

#     num_nodes = length(dz)+1

#     #calculate strength limit state capacities

#     #strong axis flexure, local-global interaction
#     Fy = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, materialProperties, 4, 3)
#     Ixx = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 1)
#     ho = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 7, 2)
#     ycy = ho./2  #distance from neutral axis to outer fiber
#     Sxx = Ixx./ycy
#     Myxx = Fy.*Sxx
#     Mnexx = Myxx
#     Mcrℓxx = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 7)

#     Mnℓxx = zeros(Float64, numberOfNodes)
#     eMnℓxx = zeros(Float64, numberOfNodes)
#     for i in eachindex(Mcrℓxx)
#         Mnℓxx[i], eMnℓxx[i] =  AISIS10016.f321(Mnexx[i], Mcrℓxx[i], ASDorLRFD)
#     end

#     #weak axis flexure, local-global interaction
#     Iyy = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 2)

#     t = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 7, 1)
#     b = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 3, 3)
#     d = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 3, 4)
#     θc = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 3, 5)

#     #distance from neutral axis to outer fiber
#     ycx = b.+d.*cos.(deg2rad.(θc)) .-t./2
#     Syy = Iyy./ycx
#     Myyy = Fy.*Syy
#     Mneyy = Myyy
#     Mcrℓyy = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 8)

#     Mnℓyy = zeros(Float64, numberOfNodes)
#     eMnℓyy = zeros(Float64, numberOfNodes)
#     for i in eachindex(Mcrℓyy)
#         Mnℓyy[i], eMnℓyy[i] = AISIS10016.f321(Mneyy[i], Mcrℓyy[i], ASDorLRFD)
#     end

#     #torsion
#     Cw = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 5)
#     Fy = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, materialProperties, 4, 3)
#     Wn = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 6)
#     Bcrℓ = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, sectionProperties, 3, 9)

#     Bn = zeros(Float64, numberOfNodes)
#     eBn = zeros(Float64, numberOfNodes)
#     for i in eachindex(Bcrℓ)
#         Bn[i], eBn[i] = AISIS10024.h411(Cw[i], Fy[i], Wn[i], Bcrℓ[i], ASDorLRFD)
#     end

#     #distortional buckling
#     CorZ = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 3, 6)
#     CorZ = trunc.(Int, CorZ)
#     E = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, materialProperties, 4, 1)
#     μ = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, materialProperties, 4, 2)
#     G= E./(2 .*(1 .+ μ))
#     f1=1.0*ones(length(numberOfNodes))
#     f2=-1.0*ones(length(numberOfNodes))
#     Lm = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, bracingProperties, 6, 3)

#     #define moment gradient factor
#     #assume it is 1
#     M1=1.0*ones(length(numberOfNodes))
#     M2=1.0*ones(length(numberOfNodes))
#     curvatureSign = -1.0*ones(length(numberOfNodes))

#     #assume clip stiffness does not help distortional buckling for a standing seam roof
#     #clips are spaced too far apart
#     kϕ=0.0*zeros(length(numberOfNodes))

#     Sf=Sxx

#     Mcrd=AISIS10016.app23331.(CorZ, t, ho, b, d, θc, E, μ, G, f1, f2, M1, M2, curvatureSign, Lm, kϕ, Sf)

#     My=Fy.*Sf

#     Mnd = zeros(Float64, numberOfNodes)
#     eMnd = zeros(Float64, numberOfNodes)
#     for i in eachindex(Mcrd)
#         Mnd[i], eMnd[i] = AISIS10016.f411(My[i], Mcrd[i], ASDorLRFD)
#     end

#     #Web shear strength
#     a = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, bracingProperties, 6, 4)
#     h = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, crossSectionDimensions, 7, 7)
#     kv  = AISIS10016.g233.(a, h)
#     Fcr = AISIS10016.g232.(E, μ, kv, h, t)
#     Vcr = AISIS10016.g231.(h, t, Fcr)

#     Vn = zeros(Float64, numberOfNodes)
#     eVn = zeros(Float64, numberOfNodes)
#     for i in eachindex(Vcr)
#         Vn[i], eVn[i] = AISIS10016.g21(E[i], h[i], t[i], Fy[i], Vcr[i], ASDorLRFD)
#     end

#     #define free flange properties
#     Iyyf = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, FlangeProperties, 3, 2)
#     xcf = Mesh.create_line_element_property_array(memberDefinitions, dm, dz, FlangeProperties, 3, 6)

#     #Free flange strength
#     #Assume yield criterion for now, no local buckling...
#     Sycf_tension = Iyyf ./abs.(xcf)
#     Sycf_comp = Iyyf ./((b .+d .*cos.(deg2rad.(θc)) .- t/2) .-abs.(xcf))
#     Sycf = min.([Sycf_tension Sycf_comp])
#     My_yyf = Fy.*Sycf
#     Mcrℓ_yyf = 999999999999999.0  #no local buckling right now

#     Mnℓyy_freeflange = zeros(Float64, numberOfNodes)
#     eMnℓyy_freeflange = zeros(Float64, numberOfNodes)
#     for i in eachindex(Mnℓyy_freeflange)
#         Mnℓyy_freeflange[i], eMnℓyy_freeflange[i] = AISIS10016.f321(My_yyf[i], Mcrℓ_yyf, ASDorLRFD)
#     end

#     #note nominal capacities are divided by ASD safety factor or
#     #multiplied by LRFD resistance factor here
#     return eMnℓxx, eMnℓyy, eBn, eMnd, eVn, eMnℓyy_freeflange

# end

# #find flexure+torsion D/C
# function bendingTorsionDemandToCapacity(Mxx, Myy, B, Myy_freeflange, eMnℓxx, eMnℓyy, eBn, eMnℓyy_freeflange)

#     #check bending + torsion interaction
#     #ActionM1, ActionM2, ActionB, Interaction
#     interactionCheck = AISIS10024.h42.(Mxx, Myy, B, Myy_freeflange, eMnℓxx, eMnℓyy, eBn, eMnℓyy_freeflange)

#     actionMxx = [x[1] for x in interactionCheck]
#     actionMyy = [x[2] for x in interactionCheck]
#     actionB = [x[3] for x in interactionCheck]
#     actionMyy_freeflange = [x[4] for x in interactionCheck]
#     totalInteraction = [x[5] for x in interactionCheck]

#     demandToCapacity = totalInteraction./1.15

#     return actionMxx, actionMyy, actionB, actionMyy_freeflange, totalInteraction, demandToCapacity

# end

# #find distortional buckling D/C
# function distortionalDemandToCapacity(Mxx,eMnd)

#     #check distortional buckling
#     demandToCapacity=abs.(Mxx./eMnd)

# end

# #find flexure+shear D/C
# function bendingShearDemandToCapacity(Vyy, Mxx, eMnℓxx, eVn)

#     interaction = AISIS10016.h21.(Mxx, Vyy, eMnℓxx, eVn)

#     demandToCapacity = interaction

#     return demandToCapacity

# end

# #find biaxial bending D/C
# function biaxialBendingDemandToCapacity(Mxx, Myy, eMnℓxx, eMnℓyy)

#     #no axial force for now
#     Pbar=zeros(length(Mxx))
#     Pa=ones(length(Mxx))

#     interactionCheck = AISIS10016.h121.(Pbar, Mxx, Myy, Pa, eMnℓxx, eMnℓyy)

#     actionP = [x[1] for x in interactionCheck]
#     actionMxx = [x[2] for x in interactionCheck]
#     actionMyy = [x[3] for x in interactionCheck]
#     totalInteraction = [x[4] for x in interactionCheck]

#     demandToCapacity = totalInteraction

#     return actionP, actionMxx, actionMyy, totalInteraction, demandToCapacity

# end

# #purlin line demand to capacity
# function lineDC(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad, eMnℓxx, eMnℓyy, eBn, eMnd, eVn, eMnℓyy_freeflange, bridging)

#     #Calculate purlin line deformation and demands.
#     z, u, v, ϕ, beamProperties = Beam.solve(memberDefinitions, sectionProperties, materialProperties, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad)
#     Mxx = InternalForces.moment(z, beamProperties.dm, -v, beamProperties.E, beamProperties.Ix)
#     Myy = InternalForces.moment(z, beamProperties.dm, -u, beamProperties.E, beamProperties.Iy)
#     Vyy = InternalForces.shear(z, beamProperties.dm, -v, beamProperties.E, beamProperties.Ix)
#     B = InternalForces.bimoment(z, beamProperties.dm, ϕ, beamProperties.E, beamProperties.Cw)

#     #Calculate free flange behavior with shear flow and flexural-torsional buckling.
#     FlangeProperties, Springs, Loads = PurlinDesigner.free_flange_define(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, bracingProperties, uniformLoad[2], Mxx)
#     uf, vf, ϕf, FreeFlangeProperties = BeamColumn.solve(memberDefinitions, FlangeProperties, materialProperties, Loads, Springs, endBoundaryConditions, bridging)
#     Myyf = InternalForces.moment(z, beamProperties.dm, -uf, FreeFlangeProperties.E, FreeFlangeProperties.Iy)

#     #Get limit state interactions and demand-to-capacities along the purlin line.
#     BTActionMxx, BTActionMyy, BTActionB, BTActionMyy_freeflange, BTTotalInteraction, BTDemandToCapacity = bendingTorsionDemandToCapacity(Mxx, Myy, B, Myyf, eMnℓxx, eMnℓyy, eBn, eMnℓyy_freeflange)
#     distDemandToCapacity = distortionalDemandToCapacity(Mxx,eMnd)
#     MVDemandToCapacity = bendingShearDemandToCapacity(Vyy, Mxx, eMnℓxx, eVn)
#     BBActionP, BBActionM1, BBActionM2, BBTotalInteraction, BBDemandToCapacity = biaxialBendingDemandToCapacity(Mxx, Myy, eMnℓxx, eMnℓyy)

#     #Find worst case demand-to-capacity ratio along the purlin line.
#     purlinDemandToCapacity=maximum([BTDemandToCapacity; distDemandToCapacity; MVDemandToCapacity; BBDemandToCapacity])

#     return purlinDemandToCapacity

# end

# #purlin line load that causes failure
# function rootfinder(q, eps, residual, demandToCapacity, memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad, eMnℓxx, eMnℓyy, eBn, eMnd, eVn, loadAngle, eMnℓyy_freeflange, bridging)

#     #use bisection method of rootfinding to solve for purlin strength

#     maxIterations=10  #hard code this for now

#     for i=1:maxIterations

#         newq=q/demandToCapacity
#         q = q + (newq-q)/2

#         qx = -q*sin(deg2rad(loadAngle))
#         qy = q*cos(deg2rad(loadAngle))
#         uniformLoad=(qx,qy)

#         demandToCapacity=lineDC(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad, eMnℓxx, eMnℓyy, eBn, eMnd, eVn, eMnℓyy_freeflange, bridging)

#         residual = 1.0 - abs(demandToCapacity)

#         if abs(residual) < eps
#             return q
#         end

#     end

#     error("Maximum rootfinding interation limit exceeded.  A failure load was not found.")

# end

# function line_strength(ASDorLRFD, gravityOrUplift, memberDefinitions, sectionProperties, crossSectionDimensions, materialProperties, loadLocation, bracingProperties, roofSlope, endBoundaryConditions, supports, bridging)

#     dz, z, dm = Mesh.define_line_element(memberDefinitions)
#     numnodes = length(z)

#     #Calculate free flange available strengths.
#     FlangeProperties, Springs, Loads = PurlinDesigner.free_flange_define_line_element(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, bracingProperties, 0.0, zeros(Float64, numnodes))

#     #Calculate purlin available strengths.
#     eMnℓxx, eMnℓyy, eBn, eMnd,  eVn, eMnℓyy_freeflange  = sectionStrengths(ASDorLRFD, dz, dm, memberDefinitions, sectionProperties, crossSectionDimensions, materialProperties, loadLocation, bracingProperties, roofSlope, FlangeProperties)

#     #Use rootfinding to solve for purlin line available strength.

#     #pick a small load
#     if gravityOrUplift==0
#       q=0.0001
#       loadAngle=roofSlope
#     elseif gravityOrUplift==1
#       q=-0.0001
#       loadAngle=0.0   #for uplift load is always perpendicular to purlin flange
#     end

#     #find DemandToCapacity for this small load
#     qx = -q*sin(deg2rad(loadAngle))
#     qy = q*cos(deg2rad(loadAngle))
#     uniformLoad = (qx,qy)
#     demandToCapacity = lineDC(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad, eMnℓxx, eMnℓyy, eBn, eMnd, eVn, eMnℓyy_freeflange, bridging)

#     #initialize residual for rootfinding
#     residual=abs(1.0-demandToCapacity)
#     eps=0.01  #residual tolerance, hard coded for now

#     #this spits out the expected failure load
#     eqn = rootfinder(q, eps, residual, demandToCapacity, memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad, eMnℓxx, eMnℓyy, eBn, eMnd, eVn, loadAngle, eMnℓyy_freeflange, bridging)

#     qx = -eqn*sin(deg2rad(loadAngle))
#     qy = eqn*cos(deg2rad(loadAngle))
#     uniformLoad = (qx,qy)

#     #calculate all the final properties and deformations for the purlin line, at failure

#     z, u, v, ϕ, beamProperties = Beam.solve(memberDefinitions, sectionProperties, materialProperties, loadLocation, bracingProperties, endBoundaryConditions, supports, uniformLoad)

#     Mxx = InternalForces.moment(z, beamProperties.dm, -v, beamProperties.E, beamProperties.Ix)
#     Myy = InternalForces.moment(z, beamProperties.dm, -u, beamProperties.E, beamProperties.Iy)
#     Vyy = InternalForces.shear(z, beamProperties.dm, -v, beamProperties.E, beamProperties.Ix)
#     T = InternalForces.torsion(z, beamProperties.dm, ϕ, beamProperties.E, beamProperties.G, beamProperties.J, beamProperties.Cw)
#     B = InternalForces.bimoment(z, beamProperties.dm, ϕ, beamProperties.E, beamProperties.Cw)

#     #Calculate free flange behavior with shear flow and flexural-torsional buckling.
#     FlangeProperties, Springs, Loads = PurlinDesigner.free_flange_define(memberDefinitions, sectionProperties, materialProperties, crossSectionDimensions, bracingProperties, uniformLoad[2], Mxx)
#     uf, vf, ϕf, FreeFlangeProperties = BeamColumn.solve(memberDefinitions, FlangeProperties, materialProperties, Loads, Springs, endBoundaryConditions, bridging)

#     Myyf = InternalForces.moment(z, beamProperties.dm, -uf, FreeFlangeProperties.E, FreeFlangeProperties.Iy)

#     #Get limit state interactions and demand-to-capacities along the purlin line.
#     BTActionMxx, BTActionMyy, BTActionB, BTActionMyy_freeflange, BTTotalInteraction, BTDemandToCapacity = bendingTorsionDemandToCapacity(Mxx, Myy, B, Myyf, eMnℓxx, eMnℓyy, eBn, eMnℓyy_freeflange)
#     distDemandToCapacity = distortionalDemandToCapacity(Mxx,eMnd)
#     MVDemandToCapacity = bendingShearDemandToCapacity(Vyy, Mxx, eMnℓxx, eVn)
#     BBActionP, BBActionMxx, BBActionMyy, BBTotalInteraction, BBDemandToCapacity = biaxialBendingDemandToCapacity(Mxx, Myy, eMnℓxx, eMnℓyy)

#     deformation = NamedTuple{(:u, :v, :ϕ, :uf, :vf, :ϕf)}((u, v, ϕ, uf, vf, ϕf))
#     strengths = NamedTuple{(:eMnℓxx, :eMnℓyy, :eBn, :eMnd, :eVn, :eMnℓyy_freeflange)}((eMnℓxx, eMnℓyy, eBn, eMnd, eVn, eMnℓyy_freeflange))
#     forces = NamedTuple{(:Mxx, :Myy, :Vyy, :T, :B, :Myyf)}((Mxx, Myy, Vyy, T, B, Myyf))
#     interactions = NamedTuple{(:BTMxx, :BTMyy, :BTB, :BTMyy_freeflange, :BTTotal, :BBP, :BBMxx, :BBMyy, :BBTotal)}((BTActionMxx, BTActionMyy, BTActionB, BTActionMyy_freeflange, BTTotalInteraction, BBActionP, BBActionMxx, BBActionMyy, BBTotalInteraction))
#     demand_to_capacity = NamedTuple{(:BT, :dist, :MV, :BB, :envelope)}((BTDemandToCapacity, distDemandToCapacity, MVDemandToCapacity, BBDemandToCapacity, demandToCapacity))

#     return eqn, z, beamProperties, deformation, strengths, forces, interactions, demand_to_capacity

# end

# function free_flange_stiffness(t, E, H, kϕc)

#     Icantilever = 1/12*t^3   #length^4/length for distributed spring

#     #Use Eq. 16 from Gao and Moen (2013) https://ascelibrary.org/doi/abs/10.1061/(ASCE)ST.1943-541X.0000860
#     #kxf = 3*E*Icantilever/H^3
#     kxf = 1/(H^2/kϕc + (H^3/(3*E*Icantilever)))

#     kϕf = E*Icantilever/H

#     return kxf, kϕf

# end

# function free_flange_define(MemberDefinitions, SectionProperties, MaterialProperties, CrossSectionDimensions, BracingProperties, q, Mxx)

#     dz, z, dm = Mesh.define_line_element(MemberDefinitions)

#     numnodes = length(z)

#     CorZ = CrossSectionDimensions[1][6] + 1
#     H = CrossSectionDimensions[1][2]
#     Bc = CrossSectionDimensions[1][3]
#     Dc = CrossSectionDimensions[1][4]
#     θc = CrossSectionDimensions[1][5]
#     t = CrossSectionDimensions[1][1]

#     r = 0.0
#     kipin = 0
#     center = 0
#     n = 5

#     node, elem = CrossSection.CZflange_template(CorZ,H,Bc,Bc,Dc,Dc,r,r,r,r,θc,θc,t,n,n,n,n,n,n,n,n,n,kipin,center)

#     coords = node[:, 2:3]
#     ends = elem[:, 2:4]

#     Af,xcf,ycf,Ixf,Iyf,Ixyf,thetaf,I1f,I2f,Jf,xsf,ysf,Cwf,B1f,B2f,wnf = CrossSection.CUFSMsection_properties(coords, ends)

#     FlangeProperties = [(Af, Ixf, Iyf, Jf, Cwf, xcf, ycf, xsf, ysf)]

#     E = MaterialProperties[1][1]   #consider generalizing this someday

#     kϕc = BracingProperties[1][2]

#     kxf, kϕf = free_flange_stiffness(t, E, H, kϕc)

#     #kx ky kϕ hx hy
#     Springs = [(kxf*ones(numnodes)),(0.0*ones(numnodes)), (kϕf*ones(numnodes)),(0.0*ones(numnodes)),(0.0*ones(numnodes))]

#     #approximate axial force in flange
#     P = -Mxx ./ ((H -t) - 2 * abs(ycf))

#     #There is shear flow in the free flange of a restrained C or Z.
#     #Use Eq. 13b and 15 from Gao and Moen (2013) https://ascelibrary.org/doi/abs/10.1061/(ASCE)ST.1943-541X.0000860

#     Ix = SectionProperties[1][1]
#     c = CrossSectionDimensions[1][8]
#     b = Bc - c

#     if CorZ == 1  #C

#         kH = ((Bc^2*t*H^2)/(4*Ix) + b)/H

#         if xcf > 0   #if flange is oriented left to right from web
#             kH = -kH
#         end

#     elseif CorZ == 2  #Z

#         kH = (H*t*(Bc^2 + 2*Dc*Bc - (2*Dc^2*Bc)/H))/(4*Ix)

#         if xcf > 0   #if flange is oriented left to right from web
#             kH = -kH
#         end

#     end

#     qx = q .* kH

#     #P qx qy ax ay
#     Loads = [P, (qx*ones(numnodes)), (0.0*ones(numnodes)),(0.0*ones(numnodes)),(ycf*ones(numnodes))]

#     return FlangeProperties, Springs, Loads

# end

end #module


#=

  Huey, Dewey and Louie problem in ConstraintSolver.jl

  From Marriott & Stuckey, Programming with Constraints, page 42
  """
  Huey, Dewey and Louie are being questioned by their uncle. These are the
  statements the make:
   Huey: Dewey and Louie has equal share in it; if one is quitly, so
         is the other.
   Dewey: If Huey is guilty, then so am I.
   Louie: Dewey and I are not both quilty.

  Their uncle, knowing that they are cub scouts, realises that they
  cannot tell a lie. Has he got sufficient information to decide who
  (if any) are quilty?
  """

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

=#

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

function huey_dewey_louie(print_solutions=true,all_solutions=true,timeout=6)

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

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

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

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

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

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

                                                            "time_limit"=>timeout,

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

                                                            # "lp_optimizer" => cbc_optimizer,
                                                            # "lp_optimizer" => glpk_optimizer,
                                                            # "lp_optimizer" => ipopt_optimizer,
                                        ))

    n = 3
    @variable(model, x[1:n], Bin)
    Huey,Dewey,Louie = x

    #  Huey: Dewey and Louie has equal share in it; if one is
    # quitly, so is the other.
    # Dewey == 1 <=> Louie == 1
    @variable(model, b1, Bin)
    @constraint(model, b1 := {Dewey ==1})
    @constraint(model, b1 := {Louie ==1})

    # Dewey: If Huey is guilty, then so am I.
    # Huey ==1 => Dewey ==1
    @variable(model, b2, Bin)
    @constraint(model, b2 := {Huey ==1})
    @constraint(model, b2 => {Dewey ==1})

    #  Louie: Dewey and I are not both quilty.
    # ~(Dewey ==1  /\ Louie ==1)
    @variable(model, b3, Bin)
    @variable(model, b4, Bin)
    @constraint(model, b3 := {Dewey == 1})
    @constraint(model, b4 := {Louie == 1})
    @constraint(model, b3+b4 < 2)

    # Solve the problem
    optimize!(model)

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

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

    return status, num_sols
end

@time huey_dewey_louie()


using Distributions, Interpolations

function simulate(
		model::CartpoleFriction,
		policy,
		K,
		z_nom, u_nom,
		Q, R,
		T_sim, Δt,
		z0, w;
        _norm = 2,
        ul = -Inf * ones(length(u_nom[1])),
        uu = Inf * ones(length(u_nom[1])),
        friction = false,
        μ = 0.1)

    T = length(K) + 1
    times = [(t-1) * Δt for t = 1:T-1]
    tf = Δt * T
    t_sim = range(0, stop = tf, length = T_sim)
    t_ctrl = range(0, stop = tf, length = T)
    dt_sim = tf / (T_sim - 1)

    p = 1:policy.output

    A_state = hcat(z_nom...)
    A_ctrl = hcat(u_nom...)

    z_rollout = [z0]
    u_rollout = []
    J = 0.0
    Jx = 0.0
    Ju = 0.0
    for tt = 1:T_sim-1
        t = t_sim[tt]
        k = searchsortedlast(times, t)

        z_cubic = zeros(model.n)
        for i = 1:model.n
            interp_cubic = CubicSplineInterpolation(t_ctrl, A_state[i,:])
            z_cubic[i] = interp_cubic(t)
        end

        z = z_rollout[end] + dt_sim * w[:,tt]
        u = eval_policy(policy, K[k], z, z_cubic, u_nom[k])

        # clip controls
        u = max.(u, ul[p])
        u = min.(u, uu[p])

        if friction
            _u = [u[1] - μ * sign(z_cubic[3]) * model.g * (model.mp + model.mc);
				  0.0;
				  0.0]
        else
            _u = u[1]
        end

        push!(z_rollout, rk3(model, z, _u, zeros(model.d), dt_sim))
        push!(u_rollout, u)

        if _norm == 2
            J += (z_rollout[end] - z_cubic)' * Q[k + 1] * (z_rollout[end] - z_cubic)

			J += (u_rollout[end] - u_nom[k][p])' * R[k][p,
                p] * (u_rollout[end] - u_nom[k][p])

			Jx += (z_rollout[end] - z_cubic)' * Q[k + 1] * (z_rollout[end] - z_cubic)

			Ju += (u_rollout[end] - u_nom[k][p])' * R[k][p,
                p] * (u_rollout[end] - u_nom[k][p])
        else
            J += norm(sqrt(Q[k + 1]) * (z_rollout[end] - z_cubic), _norm)
            J += norm(sqrt(R[k][p, p]) * (u - u_nom[k][p]), _norm)
            Jx += norm(sqrt(Q[k + 1]) * (z_rollout[end] - z_cubic), _norm)
            Ju += norm(sqrt(R[k][p, p]) * (u - u_nom[k][p]), _norm)
        end
    end
    return z_rollout, u_rollout, J / (T_sim - 1), Jx / (T_sim - 1), Ju / (T_sim - 1)
	nothing
end

# Nominal trajectories
X̄_nominal, Ū_nominal = unpack(Z̄_nominal, prob_nominal)
X̄_friction, Ū_friction = unpack(Z̄_friction, prob_friction)
X̄_dpo, Ū_dpo = unpack(Z[prob_dpo.prob.idx.nom], prob_dpo.prob.prob.nom)

# Policies
K_nominal = tvlqr(model, X̄_nominal, [Ū_nominal[t][1:1] for t = 1:T-1],
 	Q, [R[t][1:1, 1:1] for t = 1:T-1], h)
K_friction = tvlqr(model, X̄_friction, [Ū_friction[t][1:1] for t = 1:T-1],
 	Q, [R[t][1:1, 1:1] for t = 1:T-1], h)
θ = [reshape(Z[prob_dpo.prob.idx.policy[prob_dpo.prob.idx.θ[t]]],
		1, model.n) for t = 1:T-1]

# Simulation
T_sim = 10T
Δt = h
dt_sim = tf / (T_sim - 1)

W = Distributions.MvNormal(zeros(model.n), Diagonal(1.0e-5 * ones(model.n)))
w = rand(W, T_sim)

W0 = Distributions.MvNormal(zeros(model.n), Diagonal(1.0e-5 * ones(model.n)))
w0 = rand(W0, 1)

model_sim = model_friction
μ_sim = 0.1

t_sim = range(0, stop = tf, length = T_sim)

z_lqr, u_lqr, J_lqr, Jx_lqr, Ju_lqr = simulate(model_sim,
	policy,
	K_nominal,
    X̄_nominal, Ū_nominal,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

z_lqr_fr, u_lqr_fr, J_lqr_fr, Jx_lqr_fr, Ju_lqr_fr = simulate(model_sim,
	policy,
	K_friction,
    X̄_friction, Ū_friction,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

z_dpo, u_dpo, J_dpo, Jx_dpo, Ju_dpo = simulate(model_sim,
	policy,
	θ,
    X̄_dpo, Ū_dpo,
	Q, R,
	T_sim, h,
	x1 + vec(w0),
	w,
	ul = ul_friction, uu = uu_friction,
    friction = true,
    μ = μ0)

# objective value
J_lqr
J_lqr_fr
J_dpo

# state tracking
Jx_lqr
Jx_lqr_fr
Jx_dpo

# control tracking
Ju_lqr
Ju_lqr_fr
Ju_dpo


struct Mesh
    vertices::Array{Float64, 2}
    topology::Dict{Tuple{Int8, Int8}, Array{Int64, 2}}
    reference_cell
end

"""

    unit_square_mesh(nx, ny[,cell=ufl.triangle])

Build unit square mesh with lexicographic vertex ordering.

"""
function unit_square_mesh(nx::Int64, ny::Int64; cell=ufl.triangle)::Mesh
    vertices = [[x, y] for x=LinRange(0, 1, nx + 1), y=LinRange(0, 1, ny + 1)]
    vertices = collect(transpose(hcat(vertices...)))

    if cell == ufl.triangle
        cell_vert_conn = zeros(Int64, 2 * nx * ny, 3)

        for iy in 1:ny
            for ix in 1:nx
                v0 = (iy - 1) * (nx + 1) + ix
                v1 = v0 + 1
                v2 = v0 + (nx + 1)
                v3 = v1 + (nx + 1)

                c0 = 2 * ((iy - 1) * nx + ix - 1)
                cell_vert_conn[c0 + 1, :] = [v0, v1, v2]
                cell_vert_conn[c0 + 2, :] = [v1, v2, v3]
            end
        end

        ref_cell = FIAT.reference_element.ufc_cell("triangle")

    elseif cell == ufl.quadrilateral
        cell_vert_conn = zeros(Int64, nx * ny, 4)

        for iy in 1:ny
            for ix in 1:nx
                v0 = (iy - 1) * (nx + 1) + ix
                v1 = v0 + 1
                v2 = v0 + (nx + 1)
                v3 = v1 + (nx + 1)

                cell_vert_conn[(iy - 1) * nx + ix, :] = [v0, v1, v2, v3]
            end
        end

        ref_cell = FIAT.reference_element.ufc_cell("quadrilateral")
    end

    return Mesh(vertices, Dict((2, 0) => cell_vert_conn), ref_cell)
end

"""Number of mesh entities of given dimension"""
function num_entities(mesh::Mesh, dim::Int64)::Int64
    if dim == 0
        return length(mesh.vertices[:, 1])
    else
        return length(mesh.topology[(dim, 0)][:, 1])
    end
end

"""Compute connectivity between (tdim, d) and (d, 0) mesh entities"""
function compute_connectivity_tdim_d_0!(mesh::Mesh, d::Int64)
    tdim = mesh.reference_cell.get_dimension()::Int64
    cell_vertex_conn = mesh.topology[(tdim, 0)]

    # Get local (FIAT) connectivity
    fiat_connectivity = mesh.reference_cell.get_connectivity()
    ent_vert_conn_local = fiat_connectivity[(d, 0)]
    cell_ent_conn_local  = fiat_connectivity[(tdim, d)]

    ent_per_cell = length(ent_vert_conn_local)
    vertices_per_ent = length(ent_vert_conn_local[1])
    num_cells = num_entities(mesh, tdim)

    # Allocate space for global connectivities
    ent_vert_conn = zeros(Int64, num_cells * ent_per_cell, vertices_per_ent)
    cell_ent_conn = zeros(Int64, num_cells, ent_per_cell)

    for cell_idx in 1:num_cells
        for ent_idx in 1:(ent_per_cell)
            for vert_idx in 1:(vertices_per_ent)
                # Get local index for vertex on this entity
                # +1 for python --> Julia indexing shift
                vert_for_entity = ent_vert_conn_local[ent_idx][vert_idx] + 1
                # Fill global connectivities
                ent_vert_conn[(cell_idx - 1) * ent_per_cell + ent_idx, vert_idx] = cell_vertex_conn[cell_idx, vert_for_entity]
            end
        end
    end

    ent_vert_conn, cell_ent_conn = unique_inverse(ent_vert_conn)
    cell_ent_conn = reshape(cell_ent_conn, num_cells, ent_per_cell)

    mesh.topology[(tdim, d)] = cell_ent_conn
    mesh.topology[(d, 0)] = ent_vert_conn
end

function unique_inverse(arr::Array{Int64, 2})

    inverse = Dict()
    for (i, row) in enumerate(eachrow(arr))
        if haskey(inverse, row)
            # If the row is already in keys then append to the
            # indices under which it is found
            push!(inverse[row], i)
        else
            inverse[row] = [i]
        end
    end

    # Populate arrays
    unique = zeros(Int64, length(inverse), size(arr)[2])
    unique_inverse = zeros(Int64, size(arr)[1])

    for (k, (row, I)) in enumerate(inverse)
        unique[k, :] = row
        for iI in I
            unique_inverse[iI] = k
        end
    end

    return (unique, unique_inverse)
end


using Luxor, Colors

function logistic(r,x)
    r*x*(1.0-x)
end

function logisticmap(;x0=0.2,r=4,n=10)
    results = [(x0,logistic(r,x0))];
    #N = 100; Note this line should not be here
    for i in 1:(n-1) # Note change of case N -> n.  Why?
      xold = results[end][2]
      xnew = logistic(r,xold)
      append!(results,[(xold,xold)])
      append!(results,[(xold,xnew)])
  end
  results
end

simres = logisticmap(x0=0.2,r=4,n=100)

xvals = []
yvals = []

for (x, y) in simres
    append!(xvals, x)
    append!(yvals, y)
end

function cross(turtle)
    Forward(turtle, 5)
    Turn(turtle, 180)
    Forward(turtle, 10)
    Turn(turtle, 180)
    Forward(turtle, 5)
    Turn(turtle, 90)
    Forward(turtle, 5)
    Turn(turtle, 180)
    Forward(turtle, 10)
    Turn(turtle, 180)
end

turtle = Turtle()
Pencolor(turtle, "black")
Penwidth(turtle, 1.5)
Reposition(turtle, 100*simres[1][1], -100*simres[1][2])
Pendown(turtle)
for (x, y) in simres
    Reposition(turtle, 100*x, -100*y)
    #Pendown(turtle)
    #cross(turtle)
end
finish()

Drawing(500,500,"turtlegraph2.png")
origin()
background("white")

turtle = Turtle()
Pencolor(turtle, "black")
Penwidth(turtle, 1.5)
for (x, y) in simres
    Reposition(turtle, 250*x, -250*y)
    Pendown(turtle)
    cross(turtle)
end
finish()


# Author: Gabrio Rizzuti, rizzuti.gabrio@gatech.edu
# Date: August, 2020
# Copyright: Georgia Institute of Technology, 2020
#
# Run FWI on Sleipner data

## Module loading

using DrWatson
@quickactivate "UncertaintyQuantificationAVP"
using UncertaintyQuantificationAVP
using LinearAlgebra, PyPlot, Statistics, Optim
using Random; seed = 1; Random.seed!(seed)

## Loading model and synthetics

# Data
data_dict = wload(datadir("Sleipner", "Sleipner_data.bson"))
dat_n = data_dict[:dat_n]
σ2_n = data_dict[:σ2_n]
src_pars = data_dict[:src_pars]
abc_geom = data_dict[:abc_geom]
acc = data_dict[:acc_mod]

# Model
model_dict = wload(projectdir()*"/scripts/Sleipner/Sleipner_model.bson")
Mtrue = model_dict[:Mtrue]
Mbg = model_dict[:Mbg]

## Loss function

# Pre- and post-processing for log-posterior
nz = Mbg.geom.nz
dz = Mbg.geom.dz
mmin = 1f0/3000f0^2f0
mmax = Inf32
function proj_bounds(m::Array{Float32}, mmin, mmax)
    m_ = copy(m)
    m_[m_ .< mmin] .= mmin
    m_[m_ .> mmax] .= mmax
    return m_
end
prec(m::Array{Float32, 3}) = Model(Mbg.geom, proj_bounds(reshape(m, nz, :), mmin, mmax))
post(g::Model) = reshape(g.m, (nz, 1, :))

# Log-likelihood
F = seismod_fun(dat_n.pars, src_pars; abc_geom = abc_geom, acc = acc)
negloglike_fun(m::Model; compute_grad::Bool = false) = negLogLikelihood_gauss(dat_n, m, σ2_n, F; compute_grad = compute_grad)

# Log-prior
D = derivative_linop(Mbg.geom)
Mpr = Model(Mbg.geom, zeros(Float32, nz, 1))
Dx = D.eval(Mtrue-Mpr)
ϵ2 = 1f-3*norm(Dx.m, Inf)^2f0
# σ2_pr = 1f-2*sum(sqrt.(Dx.m.^2f0.+ϵ2))[1]
# neglogpr_fun(m::Model; compute_grad::Bool = false) = negLogPrior_L1smooth(m, σ2_pr, ϵ2; m0 = Mpr, D = D, compute_grad = compute_grad)
σ2_pr = 1f-3*norm(Dx)[1]^2f0
neglogpr_fun(m::Model; compute_grad::Bool = false) = negLogPrior_gauss(m, σ2_pr; m0 = Mpr, D = D, compute_grad = compute_grad)

# Log-posterior
neglogpost_fun(m::Model; compute_grad::Bool = false) = negLogPost(m, negloglike_fun, neglogpr_fun; compute_grad = compute_grad)

# Overloading log-posterior for tensor-type input+pre-/post-conditioning
function neglogpost_fun(m::Array{Float32, 3}; compute_grad::Bool = false)
    nb = size(m, 3)
    if compute_grad
        f, g_ = neglogpost_fun(prec(m); compute_grad = compute_grad)
        return sum(f)/nb, post(g_)
    else
        f = neglogpost_fun(prec(m); compute_grad = compute_grad)
        return sum(f)/nb
    end
end
function neglogpost_fun!(F, G::Union{Nothing, Array{Float32, 3}}, m::Array{Float32, 3})
    if G == nothing
        f = neglogpost_fun(m; compute_grad = false)
    else
        f, g = neglogpost_fun(m; compute_grad = true)
        G .= g
    end
    return f
end

## Optimization

M0 = deepcopy(Mbg)
method = LBFGS()
niter = 100
optimopt = Optim.Options(iterations = niter, store_trace = true, show_trace = true, show_every = 1)
result = optimize(Optim.only_fg!(neglogpost_fun!), reshape(M0.m, :, 1, 1), method, optimopt)
Mmap = prec(Optim.minimizer(result))
fval = Optim.f_trace(result)

## Saving results

invparams_dict = Dict(:σ2_n => σ2_n, :mmin => mmin, :mmax => mmax, :ϵ2 => ϵ2, :σ2_n => σ2_n, :σ2_pr => σ2_pr, :Mpr => Mpr, :M0 => M0)
results_dict = Dict(:Mmap => Mmap, :fval => fval)
wsave(datadir("Sleipner", "Sleipner_invpars.bson"), invparams_dict)
wsave(datadir("Sleipner", "Sleipner_MAP.bson"), results_dict)


using Pkg
Pkg.activate(".")
Pkg.instantiate()

using Plots
using StatsPlots
using RDatasets
using LinearAlgebra
using Query
using Statistics

iris = dataset("datasets", "iris")

@df iris scatter(
    :PetalLength,
    :PetalWidth;
    label="",
    xlabel = "Petal length",
    ylabel = "Petal width"
)


import StringUtils

function is_leap_year(year::Int64)
    if rem(year, 400) == 0
        return true
    elseif rem(year, 100) == 0
        return false
    elseif rem(year, 4) == 0
        return true
    else
        return false
    end
end

function days_in_month(month::Int64, year::Int64)
    if month == 4 || month == 6 || month == 9 || month == 11
        return 30
    elseif month == 2
        if is_leap_year(year)
            29
        else
            28
        end
    else
        31
    end
end

function main()
    day = 1
    num = 0
    for year in 1900:2000
        for month in 1:12
            day += days_in_month(month, year)
            day = rem(day, 7)
            if day == 0 # Sunday on first of the month
                if year == 1900
                    num += (month == 12) ? 1 : 0
                elseif year == 2000
                    num += (month == 12) ? 0 : 1
                else
                    num += 1
                end
            end
        end
    end
    @printf("%d Sundays fell on the first of the month during the twentieth century (1 Jan 1901 to 31 Dec 2000)\n", num)
end

main()


print("threshold: execute functions ...\n")

#threshold
x = randn(200)*2
for th in (BiggestTH(), HardTH(), SoftTH(), SemiSoftTH(), SteinTH())
    threshold(x, th, 2)
end
for th in (PosTH(), NegTH())
    threshold(x, th)
end

# denoising
vs = VisuShrink(10)
n = 2^8
x0 = testfunction(n, "Doppler")
x = x0 + 0.05*randn(n)
y = denoise(x, TI=true)
y = denoise(x, TI=true, nspin = 8)
y = denoise(x, TI=false)
y = denoise(x, nothing)
y = denoise(randn(32,32), TI=true)

# best basis
wt = wavelet(WT.db4)
x = sin(4*linspace(0,2*pi-eps(),1024))
tree = bestbasistree(x, wt)
xtb = wpt(x, wt, tree)
@test_approx_eq iwpt(xtb, wt, tree) x

x = sin(4*linspace(0,2*pi-eps(),5*64)) # non-dyadic
tree = bestbasistree(x, wt)
xtb = wpt(x, wt, tree)
@test_approx_eq iwpt(xtb, wt, tree) x

#matching pursuit
N = 128
M = 64
y=randn(N)
Arr = randn(M,N)
func(a::AbstractVector) = Arr*a
funct(a::AbstractVector) = Arr'*a
x = func(y)
matchingpursuit(x, func, funct, 0.1)


# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
# ==============================================================================
#
#   Functions related to the conversion from Euler angle and axis to DCM.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

export angleaxis_to_dcm

"""
    angleaxis_to_dcm(a::Number, v::AbstractVector)
    angleaxis_to_dcm(av::EulerAngleAxis)

Convert the Euler angle `a` [rad] and Euler axis `v` to a DCM.

Those values can also be passed inside the structure `ea` (see
[`EulerAngleAxis`](@ref)).

!!! warning
    It is expected that the vector `v` is unitary. However, no verification is
    performed inside the function. The user must handle this situation.

# Example

```jldoctest
julia> v = [1, 1, 1];

julia> v /= norm(v);

julia> angleaxis_to_dcm(pi / 2, v)
3×3 StaticArrays.SMatrix{3, 3, Float64, 9} with indices SOneTo(3)×SOneTo(3):
  0.333333   0.910684  -0.244017
 -0.244017   0.333333   0.910684
  0.910684  -0.244017   0.333333

julia> ea = EulerAngleAxis(pi / 2, v);

julia> angleaxis_to_dcm(ea)
3×3 StaticArrays.SMatrix{3, 3, Float64, 9} with indices SOneTo(3)×SOneTo(3):
  0.333333   0.910684  -0.244017
 -0.244017   0.333333   0.910684
  0.910684  -0.244017   0.333333
```
"""
@inline function angleaxis_to_dcm(a::Number, v::AbstractVector)
    # Check the arguments.
    (length(v) ≠ 3) && throw(ArgumentError("The provided vector for the Euler axis must have 3 elements."))

    sθ, cθ = sincos(a)
    aux    = 1 - cθ

    return DCM(
          cθ + v[1] * v[1] * aux,      v[1] * v[2] * aux + v[3] * sθ, v[1] * v[3] * aux - v[2] * sθ,
        v[1] * v[2] * aux - v[3] * sθ,   cθ + v[2] * v[2] * aux,      v[2] * v[3] * aux + v[1] * sθ,
        v[1] * v[3] * aux + v[2] * sθ, v[2] * v[3] * aux - v[1] * sθ,   cθ + v[3] * v[3] * aux
    )'
end

@inline angleaxis_to_dcm(av::EulerAngleAxis) = angleaxis_to_dcm(av.a, av.v)


using ModelConstructors, FileIO, Random, SMC, JLD2

### Construct a generic model and populate it with parameters
reg = GenericModel()
fn = dirname(@__FILE__)
reg <= Setting(:dataroot, "$(fn)/save/input_data/")
reg <= Setting(:saveroot, "$(fn)/save/")

reg <= parameter(:α1, 0., (-1e5, 1e5), (-1e5, 1e5), Untransformed(), Normal(0, 10), fixed = false)
reg <= parameter(:β1, 0., (-1e5, 1e5), (-1e5, 1e5), Untransformed(), Normal(0, 10), fixed = false)

### Estimate with SMC

## Make data
N = 100 # number observations
M = 1 # number regressors

X = rand(N)
β = 1.
α = 1.
σ2 = 1.
y = β*X .+ α #.+ randn(N) .* sqrt(σ2)

jldopen(dataroot(reg)*"reg_data.jld2", "w") do file
    file["data"] = hcat(y, X)
end

function compute_coefficients(X::Vector, Y::Vector)
    @assert length(X) == length(Y)
    X_aug = hcat(ones(length(X)), X)
    β     = inv(X_aug'* X_aug)*X_aug'*Y
    return β
end

function compute_coefficients(X::Array, Y::Vector)
    @assert size(X)[1] == length(Y)
    X_aug = hcat(ones(size(X)[1], 1), X)
    β     = inv(X_aug'* X_aug)*X_aug'*Y
    return β
end

compute_coefficients(X, y)

## Construct log-likelihood function:
function log_likelihood_fnct(p, y)
    α = p[1]
    β = p[2]
    term1 = -(N/2)*log(2*π) - (N/2)*log(σ2)
    errors = y .- α .- β .* X
    logprob = term1 - (1/(2*σ2))*dot(errors, errors)
    return logprob
end

Random.seed!(1793)
println("Starting to estimate Regression with SMC . . .")
@everywhere using SMC
smc(log_likelihood_fnct, reg.parameters, Matrix(Matrix{Float64}((y'))'), n_parts = 100, use_fixed_schedule = true) #, tempering_target = 0.99)

cloud = load("smc_cloud.jld2", "cloud")
mean(SMC.get_vals(cloud), dims = 2)


using CUSPARSE
using CUDArt
using Base.Test

m = 25
n = 35
k = 10
blockdim = 5

@testset "doti" begin
    @testset for elty in [Float32,Float64,Complex64,Complex128]
        x = sparsevec(rand(1:m,k), rand(elty,k), m)
        y = rand(elty,m)
        d_x = CudaSparseVector(x)
        d_y = CudaArray(y)
        ddot = CUSPARSE.doti(d_x,d_y,'O')
        #compare
        dot = zero(elty)
        for i in 1:length(x.nzval)
            dot += x.nzval[i] * y[x.nzind[i]]
        end
        @test ddot ≈ dot
    end
end

@testset "dotci" begin
    @testset for elty in [Complex64,Complex128]
        x = sparsevec(rand(1:m,k), rand(elty,k), m)
        y = rand(elty,m)
        d_x = CudaSparseVector(x)
        d_y = CudaArray(y)
        ddot = CUSPARSE.dotci(d_x,d_y,'O')
        #compare
        dot = zero(elty)
        for i in 1:length(x.nzval)
            dot += conj(x.nzval[i]) * y[x.nzind[i]]
        end
        @test ddot ≈ dot
    end
end


function initialprobs(
    lower::Array{T,1},
    upper::Array{T,1};
    maxsamples = 10000,
) where {T<:Number}
    p = length(lower)
    @assert p == length(upper)

    bitlen = p * 32
    mat = Array{Int8,2}(undef, maxsamples, bitlen)
    for tries = 1:maxsamples
        randvalues = map((x, y) -> x + rand() * (y - x), lower, upper)
        mat[tries, :] .= bits(randvalues)
    end
    return colmeans(mat)
end

function sample(probs::Array{T,1}) where {T<:Number}
    return map(x -> if rand() < x
        1
    else
        0
    end, probs)
end

function mccga(;
    lower::Array{T,1},
    upper::Array{T,1},
    costfunction::Function,
    popsize::Int,
    maxsamples = 10000,
) where {T<:Number}
    probvector = initialprobs(lower, upper, maxsamples = maxsamples)
    initial_probvector = copy(probvector)
    chsize = length(probvector)
    mutation = 1.0 / convert(Float64, popsize)
    numiters = 0
    while !(all(x -> (x <= mutation) || (x >= 1.0 - mutation), probvector))
        numiters += 1
        ch1 = sample(probvector)
        ch2 = sample(probvector)
        cost1 = costfunction(floats(ch1))
        cost2 = costfunction(floats(ch2))
        winner = ch1
        loser = ch2
        if (cost2 < cost1)
            winner = ch2
            loser = ch1
        end
        for i = 1:chsize
            if winner[i] != loser[i]
                if winner[i] == 1
                    probvector[i] += mutation
                else
                    probvector[i] -= mutation
                end
                if probvector[i] > 1
                    probvector[i] = 1
                elseif probvector[i] < 0
                    probvector[i] = 0
                end
            end
        end
    end

    sampledvector = sample(probvector)
    initial_solution = floats(sampledvector)

    local_result = Optim.optimize(costfunction, initial_solution, Optim.NelderMead())

    resultdict = Dict(
        "initial_prob_vector" => initial_probvector,
        "final_prob_vector" => sampledvector,
        "iterations" => numiters,
        "initial_solution" => floats(sampledvector),
        "final_solution" => local_result.minimizer,
        "initial_minimum" => costfunction(floats(sampledvector)),
        "final_minimum" => costfunction(local_result.minimizer),
    )

    return resultdict
end


"""
    hubbardMPO(t, U, pchs, N[; T = ComplexF64])
returns a `U1`-symmetric MPO for the Hubbard model
```
    H = -∑_i t (a^†_i a_(i+1) + a_i a^†_(i+1)) + U/2 n_i (n_i-1)
```
on N sites.
"""
function hubbardMPO(t, U, pchs, N; T = ComplexF64)
    pdims = fill(1, length(pchs))
    bulk = DASTensor{T,4}(
        U1(),
        (U1Charges(-1:1), pchs, pchs, U1Charges(-1:1)),
        ([1,2,1], copy(pdims), copy(pdims), [1,2,1]),
        InOut(1,1,-1,-1))
    initwithzero!(bulk)
    preapp  = T[0 0; 0 1] # |0><0|_p ⊗ |1><1|_d
    postapp = T[1 0; 0 0] # |0><0|_p ⊗ |0><0|_d
    app     = T[0 0; 1 0] # |0><0|_p ⊗ |1><0|_d
    mat00to1 = reshape(T[0, 1], 2, 1)
    mat1to01 = reshape(T[1, 0], 1, 2)

    for u1ch in pchs
        ch = u1ch.ch
        peye= Matrix{T}(I, 1, 1)
        #local interaction term
        @tensor deg[1, 2, 3, 4] := preapp[1, 4]  * peye[2, 3] +  # |1><1| * Ι
                                   postapp[1, 4] * peye[2, 3] +  # |0><0| * Ι
                                   U/2 * ch * (ch-1)  * app[1, 4] * peye[2, 3]
                                   # U * ch * (ch-1) * |1><0|
        bulk[DASSector(U1Charge(0), U1Charge(ch), U1Charge(ch), U1Charge(0))] = deg

        #hopping term
        U1Charge(ch-1) in pchs || continue
        #a a^†
        # (0,0) → a_i → (1,0)
        @tensor deg[1, 2, 3, 4] := mat00to1[1, 4] * peye[2, 3]
        bulk[DASSector(U1Charge(0), U1Charge(ch), U1Charge(ch-1), U1Charge(1))] = -t*deg
        # (1,0) → a^†_{i+1} → (0,1)
        @tensor deg[1, 2, 3, 4] := mat1to01[1, 4] * peye[2, 3]
        bulk[DASSector(U1Charge(1), U1Charge(ch-1), U1Charge(ch), U1Charge(0))] = deg

        #a^† a
        # (0,0) → a^†_i → (-1,0)
        @tensor deg[1, 2, 3, 4] := mat00to1[1, 4] * peye[2, 3]
        bulk[DASSector(U1Charge(0), U1Charge(ch-1), U1Charge(ch), U1Charge(-1))] = -t*deg
        # (-1,0) → a_{i+1} → (0,1)
        @tensor deg[1, 2, 3, 4] := mat1to01[1, 4] * peye[2, 3]
        bulk[DASSector(U1Charge(-1), U1Charge(ch), U1Charge(ch-1), U1Charge(0))] = deg
    end
    # (3)        (4)       (3)
    # [lb](2) (2)[b](3) (2)[rb]
    # (1)        (1)       (1)
    lb = DASTensor{T,1}(U1(), (U1Charges(-1:1),), ([1,2,1],), InOut(-1))
    rb = DASTensor{T,1}(U1(), (U1Charges(-1:1),), ([1,2,1],), InOut( 1))
    lb[DASSector(U1Charge(0))] = [0, 1]
    rb[DASSector(U1Charge(0))] = [1, 0]

    @tensor bulk[1,2,3,4]       := bulk[2,1,4,3]
    @tensor leftbound[1, 2, 3]  := lb[-1] *  bulk[1, -1, 2, 3]
    @tensor rightbound[1, 2, 3] := bulk[1, 2, -1, 3] * rb[-1]

    return MPO{N}(leftbound, bulk,  rightbound)
end

"""
    creationOp(pchs::U1Charges [, T = ComplexF64])
return the creation operator `a†` acting on charges `pchs`
"""
function creationOp(pchs::U1Charges, T = ComplexF64)
    a = DASTensor{T,2}(U1(), (pchs, pchs),
        (fill(1, length(pchs)), fill(1, length(pchs))),
        InOut(1,-1))
    initwithzero!(a)
    for ch in pchs
        nch = ch ⊕ U1Charge(-1)
        nch in pchs || continue
        a[DASSector(nch, ch)] = reshape([sqrt(ch.ch)],1,1)
    end
    return a
end

"""
    annihilationOp(pchs::U1Charges [, T = ComplexF64])
return the annihilation operator `a` acting on charges `pchs`
"""
function annihilationOp(pchs::U1Charges, T = ComplexF64)
    a = DASTensor{T,2}(U1(), (pchs, pchs),
        (fill(1, length(pchs)), fill(1, length(pchs))),
        InOut(1,-1))
    initwithzero!(a)
    for ch in pchs
        nch = ch ⊕ U1Charge(-1)
        nch in pchs || continue
        a[DASSector(ch, nch)] = reshape([sqrt(ch.ch)],1,1)
    end
    return a
end


using GeoPhyInv
using BenchmarkTools
using LinearAlgebra
using Test
GInterp=GeoPhyInv.Interpolation

function dptest(nx,nz,mx,mz,Battrib)
	pa=GInterp.Kernel([nx, nz], [mx, mz],Battrib)
	ny=randn(length(nz), length(nx));
	my=zeros(length(mz), length(mx));
	# interp
	@time GInterp.interp_spray!(ny, my, pa, :interp)
	myp = randn(size(my));
	# spray
	nyp=zeros(length(nz), length(nx));
	@time GInterp.interp_spray!(nyp, myp, pa, :spray)
	# dot product test
	@test LinearAlgebra.dot(my, myp) ≈ LinearAlgebra.dot(ny, nyp)
end

@testset "GInterp to a larger grid using  nx==1 or nz==1" begin
	nx=range(1.,step=20.,length=1);
	mx=range(1.,stop=40.,length=10);
	nz=range(1.,stop=20.,length=30);
	mz=range(4.,stop=16.,length=30);

	dptest(nx,nz,mx,mz,:B1)

	nx=range(4.,stop=16.,length=20);
	mx=range(1.,stop=30.,length=30);
	nz=range(1.,step=5.,length=1);
	mz=range(1.,stop=10.,length=66);

	dptest(nx,nz,mx,mz,:B1)
end

@testset "GInterp on same grid for nx==1 or nz==1" begin
	nx=range(1.,stop=1.,length=1);
	mx=range(1.,stop=1.,length=1);
	nz=range(1.,stop=20.,length=30);
	mz=range(4.,stop=16.,length=30);

	dptest(nx,nz,mx,mz,:B1)

	nx=range(4.,stop=16.,length=20);
	mx=range(1.,stop=30.,length=30);
	nz=range(1.,stop=1.,length=1);
	mz=range(1.,stop=1.,length=1);

	dptest(nx,nz,mx,mz,:B1)
end

@testset "no extrapolations" begin
	nx=range(21,stop=100,length=80);
	mx=range(1,stop=100,length=100);
	ny=ones(length(nx));
	c=randn()
	my=c.*ones(length(mx))

	pa=GInterp.P_core([nx], [mx], :B1)

	GInterp.interp_spray!(ny, my, pa, :interp)
	@test all(my[1:20] .== c)

	nx=range(3,stop=7,length=64);
	nz=range(3,stop=7,length=95);
	mx=range(1,stop=10,length=10);
	mz=range(1,stop=10,length=10);
	ny=ones(length(nz), length(nx));
	c=randn()
	my=c.*ones(length(mz), length(mx))

	pa=GInterp.P_core([nx, nz], [mx, mz], :B1)

	GInterp.interp_spray!(ny, my, pa, :interp)

	@test all(my[my .≠1] .== c)

end

# also testing behaviour when out of bounds!!
nx=range(1,stop=20,length=100);
mx=range(3,stop=25,length=200);
nz=range(1,stop=20,length=30);
mz=range(5,stop=27,length=40);

println("=====================================")
for Battrib in [:B1, :B2]
	global nx, mx

	pa=GInterp.Kernel([nx], [mx], Battrib)
	println("=====================================")
	## 1D
	ny=randn(length(nx));
	# interpolate (my=f(ny))
	my=zeros(length(mx));
	@time GInterp.interp_spray!(ny, my, pa, :interp)

	myp = randn(length(my));

	# spray (nyp=fˣ(myp))
	nyp=zeros(length(nx));
	@time GInterp.interp_spray!(nyp, myp, pa, :spray)

	# dot product test
	@test LinearAlgebra.dot(my, myp) ≈ LinearAlgebra.dot(ny, nyp)
end

@testset "GInterp on the same grid should not change for B1" begin
	Battrib=:B1
	pa=GInterp.Kernel([nx, nz], [nx, nz], Battrib)
	println("=====================================")
	ny=randn(length(nz), length(nx));
	my=zeros(length(nz), length(nx));
	@time GInterp.interp_spray!(ny, my, pa, :interp)
	@test ny≈my

	nyvec=vcat(vec(ny),vec(ny))
	myvec=zeros(length(my)*2)
	@time GInterp.interp_spray!(nyvec, myvec, pa, :interp, 2)
	@test nyvec≈myvec

	myp = randn(size(my));
	nyp=zeros(length(nz), length(nx));
	@time GInterp.interp_spray!(nyp, myp, pa, :spray)
	@test LinearAlgebra.dot(my, myp) ≈ LinearAlgebra.dot(ny, nyp)
end

for Battrib in [:B1, :B2]
	global nx, nz, mx, mz
	pa=GInterp.Kernel([nx, nz], [mx, mz], Battrib)
	## 2D
	ny=randn(length(nz), length(nx));
	# interpolate
	my=zeros(length(mz), length(mx));
	@time GInterp.interp_spray!(ny, my, pa, :interp)

	myp = randn(size(my));

	# spray
	nyp=zeros(length(nz), length(nx));
	@time GInterp.interp_spray!(nyp, myp, pa, :spray)

	# dot product test
	@test LinearAlgebra.dot(my, myp) ≈ LinearAlgebra.dot(ny, nyp)

	nmod=3
	ny=randn(length(nz)*length(nx)*nmod);
	# interpolate
	my=zeros(length(mz)*length(mx)*nmod);
	@time GInterp.interp_spray!(ny, my, pa, :interp,nmod)

	myp = randn(size(my));

	# spray
	nyp=zeros(length(nz)*length(nx)*nmod);
	@time GInterp.interp_spray!(nyp, myp, pa, :spray,nmod)

	# dot product test
	@test LinearAlgebra.dot(my, myp) ≈ LinearAlgebra.dot(ny, nyp)

end


"
 Projected newton QP solver

 author: Atsushi Sakai

 Ref:

 - [Control-limited differential dynamic programming](https://homes.cs.washington.edu/~todorov/papers/TassaICRA14.pdf)

"

using Statistics
using LinearAlgebra
const __MAIN__ = length(PROGRAM_FILE)!=0 && occursin(PROGRAM_FILE, @__FILE__)

export solve_qp_with_projected_newton

"""
    Solve quadratic programming with projected newton method

    argmin(0.5*x'*H*x + x'*g)
    s.t. lower<=x<=upper

    inputs:
        H - positive definite matrix   (n * n)
        g - a vector                   (n)
        lower - lower bounds           (n)
        upper - upper bounds           (n)

    optional inputs:
        x0       - initial state       (n)
        maxIter        = 100       maximum number of iterations
        minGrad        = 1e-8      minimum norm of non-fixed gradient
        minRelImprove  = 1e-8      minimum relative improvement
        stepDec        = 0.6      factor for decreasing stepsize
        minStep        = 1e-22    minimal stepsize for linesearch
        Armijo         = 0.1      Armijo parameter
        verbose        = false    verbosity

    outputs:
        xstar    - solution            (n)
        status   - status dictionary
"""
function solve_qp_with_projected_newton(H, g, lower, upper;
           maxIter = 100, minGrad = 1e-8, minRelImprove = 1e-8,
           stepDec = 0.6, minStep = 1e-22, Armijo = 0.1, verbose = false
           )

    n        = size(H,1)
    clamped  = falses(n,1)
    free     = trues(n,1)
    oldvalue = 0.0
    result   = -10
    gnorm    = 0.0
    nfactor  = 0
    Hfree    = zeros(n)

    # initialize state
    LU = [lower upper]
    x = vec(mean(LU, dims=2))
    x[x.==-Inf].=0
    x[x.== Inf].=0

    # initial objective value
    value    = x'*g + 0.5*x'*H*x

    if verbose
        println("==========")
        println("Starting box-QP, dimension ", n, ", initial cost:",value)
    end

    niter = 0
    for iter = 1:maxIter
        niter = iter

        # check relative improvement
        if( iter>1 && (oldvalue - value) < minRelImprove*abs(oldvalue) )
            result = 4
            break
        end
        oldvalue = value #update cost

        grad = g + H*x # calc gradient

        # find clamped dimensions
        old_clamped = clamped
        clamped     = falses(n,1)
        clamped[(x .== lower).&(grad.>0)].=true
        clamped[(x .== upper).&(grad.<0)].= true
        free = .~clamped

        # check for all clamped
        if all(clamped)
            result = 6
            break
        end

        # factorize if clamped has changed
        if iter == 1
            factorize = true
        else
            factorize = any(old_clamped != clamped);
        end

        f_ind = [i for i in 1:length(free) if free[i]]

        if factorize
            cH = LinearAlgebra.cholesky(H[f_ind,f_ind])
            Hfree = cH.U
            nfactor += 1
        end

        # check gradient norm
        gnorm  = norm(grad[f_ind])
        if gnorm < minGrad
            result = 5
            break
        end

        # get search direction
        grad_clamped   = g  + H*(x.*clamped)
        search         = zeros(n,1)
        search[f_ind]   = -Hfree\(Hfree'\grad_clamped[f_ind]) .- x[f_ind];

        # check for descent direction
        sdotg = sum(search.*grad)
        if sdotg >= 0 # (should not happen)
            result = 0
            break
        end

        # armijo linesearch
        step  = 1.0
        nstep = 0
	    xc    = clamping(x+step*search, upper, lower)
        vc    = xc'*g + 0.5*xc'*H*xc

        while ((vc - oldvalue)/(step*sdotg)) < Armijo
            step  *= stepDec
            nstep += 1
            xc    = clamping(x+step*search, upper, lower)
            vc    = xc'*g + 0.5*xc'*H*xc
            if step<minStep
                break
            end
        end

        if verbose
            println("iter:",iter," value:", vc[1], " |g|:", gnorm,
                    " reduction:", (oldvalue-vc)[1], " linesearch:",
                    stepDec, " n_clamped", nstep, sum(clamped))
        end

        # accept candidate
        x = xc
        value = vc
    end

    if niter >= maxIter
        result = 1
    end

    msgs = Dict([(-1, "Hessian is not positive definite"),
                (0, "No descent direction found"),
                (1, "Maximum main iterations exceeded"),
                (2, "Maximum line-search iterations exceeded"),
                (3, "No bounds, returning Newton point"),
                (4, "Improvement smaller than tolerance"),
                (5, "Gradient norm smaller than tolerance"),
                (6, "All dimensions are clamped")
                ])

    status = Dict([ ("objectives", value[1]),
                    ("residuals", gnorm),
                    # ("free", free),
                    ("iter", niter),
                    ("result", msgs[result]),
                  ])

    if result>=3
        status["optimal"] = true # optimization succeeded
    else
        status["optimal"] = false # optimization failed
    end

    if verbose
        println("RESULT:", result, " iterations:", niter, " gradient:", gnorm, " final cost:", value[1], " factorization:", nfactor)
    end

	return x, status
end

function clamping(x,upper,lower)
    minl = [minimum([ui, xi]) for (ui, xi) in zip(upper,x)]
    return [maximum([li, xi]) for (li,xi) in zip(lower, minl)]
end

function main()
    println(PROGRAM_FILE," start!!")

    n 		= 5
    g 		= randn(n)
    println("g:")
    display(g)
    println("")
    H 		= randn(n,n)
    H 		= H*H'
    println("H:")
    display(H)
    println("")
    lower 	= -ones(n)
    lower[3] = -Inf
    println("lower",lower)
    display(lower)
    upper 	=  ones(n)
    upper[1] = Inf
    println("upper",upper)

    @time xstar, status = solve_qp_with_projected_newton(H, g, lower, upper)
    print(status)
    println(xstar)

    println(PROGRAM_FILE," Done!!")
end

function main2()
    println(PROGRAM_FILE," start!!")

    n 		= 3
    q = [3.0;4.0;-5.0]
    println("q:")
    display(q)
    println("")
    A = [1.0 0.0 -2.0;
         1.0 2.0 0.0;
         0.0 0.0 3.0]
    A = A*A'
    println("A:")
    display(A)
    println("")
    xmin = [-3.0;-5.0;-2.0]
    display(xmin)
    xmax = [1.0;1.0;1.0]
    println("lower",xmin)
    println("upper",xmax)

    xstar, status = solve_qp_with_projected_newton(A, q, xmin, xmax, verbose=true)
    println(xstar)
    print(status)

    println(PROGRAM_FILE," Done!!")
end

if __MAIN__
    # @time main()
    @time main2()
end


module OptimTools

	using KrylovMethods

	include("checkDerivative.jl")
	include("lineSearch.jl")
	include("newton.jl")
	include("dampedNewton.jl")
	include("sd.jl")
	include("bfgs.jl")
	include("nlcg.jl")
	include("modnewton.jl")
	include("newtoncg.jl")
	include("sr1.jl")
	include("tr.jl")
end


export fitness, hamming_max, hamming_raw

# Evaluates the fitness of a particular chromosome relative to a
# particular goal. The sum of the Hamming distances for each output
# relative to the goal for that output is passed through the `h2f`
# function to convert it into a useful fitness value. Valid `h2f`
# functions should always result in higher values for more "fit"
# candidates. In other words, bigger should always be better. Beyond
# that, the result only need be numeric.

function fitness{N}(c::Chromosome, g::Goal{N}, h2f::Function)
    outputs = execute_chromosome(c)
    dist = 0
    for i = 1:N
        dist += count_ones(outputs[i] $ g.truth_table[i])
    end
    return h2f(dist)
end

fitness(c::Chromosome, g::Goal) = fitness(c, g, hamming_max)

hamming_max(dist) = 1.0 / (1.0 + dist)
hamming_neg(dist) = -dist


"""
    glcm = glcm(img, distance, angle, mat_size=16)
    glcm = glcm(img, distances, angle, mat_size=16)
    glcm = glcm(img, distance, angles, mat_size=16)
    glcm = glcm(img, distances, angles, mat_size=16)

Calculates the GLCM (Gray Level Co-occurence Matrix) of an image. The `distances` and `angles` arguments may be
a single integer or a vector of integers if multiple GLCMs need to be calculated. The `mat_size` argument is used
to define the granularity of the GLCM.
"""
function glcm(img::AbstractArray{T, 2}, distance::Integer, angle::Real, mat_size::Integer = 16) where T<:Real
    max_img = maximum(img)
    img_rescaled = map(i -> max(1, Int(ceil(i * mat_size / max_img))), img)
    _glcm(img_rescaled, distance, angle, mat_size)
end

function glcm(img::AbstractArray{T, 2}, distances::Array{Int, 1}, angles::Array{A, 1}, mat_size::Integer = 16) where {T<:Real, A<:Real}
    max_img = maximum(img)
    img_rescaled = map(i -> max(1, Int(ceil(i * mat_size / max_img))), img)
    glcm_matrices = [_glcm(img_rescaled, d, a, mat_size) for d in distances, a in angles]
    glcm_matrices
end

function glcm(img::AbstractArray{T, 2}, distance::Integer, angle::Real, mat_size::Integer = 16) where T<:Colorant
    img_rescaled = map(i -> max(1, Int(ceil((reinterpret(gray(convert(Gray{N0f8}, i)))) * mat_size / 256))), img)
    _glcm(img_rescaled, distance, angle, mat_size)
end

function glcm(img::AbstractArray{T, 2}, distances::Array{Int, 1}, angles::Array{A, 1}, mat_size::Integer = 16) where {T<:Colorant, A<:Real}
    img_rescaled = map(i -> max(1, Int(ceil((reinterpret(gray(convert(Gray{N0f8}, i)))) * mat_size / 256))), img)
    glcm_matrices = [_glcm(img_rescaled, d, a, mat_size) for d in distances, a in angles]
    glcm_matrices
end

glcm(img::AbstractArray{T, 2}, distances::Int, angles::Array{A, 1}, mat_size::Integer = 16) where {T<:Union{Colorant, Real}, A<:Real} = glcm(img, [distances], angles, mat_size)
glcm(img::AbstractArray{T, 2}, distances::Array{Int, 1}, angles::Real, mat_size::Integer = 16) where {T<:Union{Colorant, Real}} = glcm(img, distances, [angles], mat_size)

function _glcm(img::AbstractArray{T, 2}, distance::Integer, angle::Number, mat_size::Integer) where T
    co_oc_matrix = zeros(Int, mat_size, mat_size)
    sin_angle = sin(angle)
    cos_angle = cos(angle)
    for j = 1:size(img, 1)
        j_off = j + Int(round(sin_angle * distance))
        checkbounds(Bool, img, j_off, 1) || continue
        for i = 1:size(img, 2)
            int_one = img[j, i]
            i_off = i + Int(round(cos_angle * distance))

            if checkbounds(Bool, img, j_off, i_off)
                int_two = img[j_off, i_off]
                co_oc_matrix[int_one, int_two] += 1
            end
        end
    end
    co_oc_matrix
end

"""
    glcm = glcm_symmetric(img, distance, angle, mat_size=16)
    glcm = glcm_symmetric(img, distances, angle, mat_size=16)
    glcm = glcm_symmetric(img, distance, angles, mat_size=16)
    glcm = glcm_symmetric(img, distances, angles, mat_size=16)

Symmetric version of the [`glcm`](@ref) function.
"""
function glcm_symmetric(img::AbstractArray, distance::Integer, angle::Real, mat_size=16)
    co_oc_matrix = glcm(img, distance, angle, mat_size)
    co_oc_matrix_trans = co_oc_matrix'
    co_oc_matrix_symm = co_oc_matrix + co_oc_matrix_trans
    co_oc_matrix_symm
end

function glcm_symmetric(img::AbstractArray, distances, angles, mat_size=16)
    co_oc_matrices = glcm(img, distances, angles, mat_size)
    co_oc_matrices_sym = map(gmat -> gmat + gmat', co_oc_matrices)
    co_oc_matrices_sym
end

"""
    glcm = glcm_norm(img, distance, angle, mat_size)
    glcm = glcm_norm(img, distances, angle, mat_size)
    glcm = glcm_norm(img, distance, angles, mat_size)
    glcm = glcm_norm(img, distances, angles, mat_size)

Normalised version of the [`glcm`](@ref) function.
"""
function glcm_norm(img::AbstractArray, distance::Integer, angle::Real, mat_size=16)
    co_oc_matrix = glcm(img, distance, angle, mat_size)
    co_oc_matrix_norm = co_oc_matrix / Float64(sum(co_oc_matrix))
    co_oc_matrix_norm
end

function glcm_norm(img::AbstractArray, distances, angles, mat_size=16)
    co_oc_matrices = glcm(img, distances, angles, mat_size)
    co_oc_matrices_norm = map(gmat -> gmat /= Float64(sum(gmat)), co_oc_matrices)
    co_oc_matrices_norm
end

"""
Multiple properties of the obtained GLCM can be calculated by using the `glcm_prop` function which calculates the
property for the entire matrix. If grid dimensions are provided, the matrix is divided into a grid and the property
is calculated for each cell resulting in a height x width property matrix.
```julia
prop = glcm_prop(glcm, property)
prop = glcm_prop(glcm, height, width, property)
```
Various properties can be calculated like `mean`, `variance`, `correlation`, `contrast`, `IDM` (Inverse Difference Moment),
 `ASM` (Angular Second Moment), `entropy`, `max_prob` (Max Probability), `energy` and `dissimilarity`.
"""
function glcm_prop(gmat::Array{T, 2}, window_height::Integer, window_width::Integer, property::Function) where T<:Real
    k_h = Int(floor(window_height / 2))
    k_w = Int(floor(window_width / 2))
    glcm_size = size(gmat)
    R = CartesianIndices(glcm_size)
    prop_mat = zeros(Float64, glcm_size)
    for I in R
        prop_mat[I] = property(gmat[max(1, I[1] - k_h) : min(glcm_size[1], I[1] + k_h), max(1, I[2] - k_w) : min(glcm_size[2], I[2] + k_w)])
    end
    prop_mat
end

glcm_prop(gmat::Array{T, 2}, window_size::Integer, property::Function) where {T<:Real} = glcm_prop(gmat, window_size, window_size, property)

function glcm_prop(gmat::Array{T, 2}, property::Function) where T<:Real
    property(gmat)
end

function contrast(glcm_window::Array{T, 2}) where T<:Real
    sum([(id[1] - id[2]) ^ 2 * glcm_window[id] for id in CartesianIndices(size(glcm_window))])
end

function dissimilarity(glcm_window::Array{T, 2}) where T<:Real
    sum([glcm_window[id] * abs(id[1] - id[2]) for id in CartesianIndices(size(glcm_window))])
end

function glcm_entropy(glcm_window::Array{T, 2}) where T<:Real
    -sum(map(i -> i * log(i), glcm_window))
end

function ASM(glcm_window::Array{T, 2}) where T<:Real
    sum(map(i -> i ^ 2, glcm_window))
end

function IDM(glcm_window::Array{T, 2}) where T<:Real
    sum([glcm_window[id] / (1 + (id[1] - id[2]) ^ 2) for id in CartesianIndices(size(glcm_window))])
end

function glcm_mean_ref(glcm_window::Array{T, 2}) where T<:Real
    sumref = sum(glcm_window, dims=2)
    meanref = sum([id * sumref[id] for id = 1:size(glcm_window)[1]])
    meanref
end

function glcm_mean_neighbour(glcm_window::Array{T, 2}) where T<:Real
    sumneighbour = sum(glcm_window, dims=1)
    meanneighbour = sum([id * sumneighbour[id] for id = 1:size(glcm_window)[2]])
    meanneighbour
end

function glcm_var_ref(glcm_window::Array{T, 2}) where T<:Real
    mean_ref = glcm_mean_ref(glcm_window)
    sumref = sum(glcm_window, dims=2)
    var_ref = sum([((id - mean_ref) ^ 2) * sumref[id] for id = 1:size(glcm_window)[1]])
    var_ref ^ 0.5
end

function glcm_var_neighbour(glcm_window::Array{T, 2}) where T<:Real
    mean_neighbour = glcm_mean_neighbour(glcm_window)
    sumneighbour = sum(glcm_window, dims=1)
    var_neighbour = sum([((id - mean_neighbour) ^ 2) * sumneighbour[id] for id = 1:size(glcm_window)[2]])
    var_neighbour ^ 0.5
end

function correlation(glcm_window::Array{T, 2}) where T<:Real
    mean_ref = glcm_mean_ref(glcm_window)
    var_ref = glcm_var_ref(glcm_window)
    mean_neighbour = glcm_mean_neighbour(glcm_window)
    var_neighbour = glcm_var_neighbour(glcm_window)
    if var_ref == 0 || var_neighbour == 0
        return 1
    end
    sum([glcm_window[i, j] * (i - mean_ref) * (j - mean_neighbour) for i = 1:size(glcm_window)[1], j = 1:size(glcm_window)[2]]) / (var_ref * var_neighbour)
end

function max_prob(glcm_window::Array{T, 2}) where T<:Real
    maxfinite(glcm_window)
end

function energy(glcm_window::Array{T, 2}) where T<:Real
    ASM(glcm_window) ^ 0.5
end


function dijkstra(g::AbstractGraph, start::Vertex)
    # clear vertex marks
    g.mark = g.mark * 0

    # init distance vector
    distance = fill(Inf, countvertices(g))
    distance[start] = 0

    for i = 1:countvertices(g)
        v = minimumvertex!(g, distance)
        (distance[v] == Inf) && throw(ErrorException("Unreachable vertices."))
        setmark!(g, v, 1)
        let w = first(g, v)
            while w <= countvertices(g)
                newdistance = distance[v] + getweight(g, v, w)
                if distance[w] > newdistance
                    distance[w] = newdistance
                end
                w = next(g, v, w)
            end
        end
    end

    # clear vertex marks
    g.mark = g.mark * 0
    return distance
end


module MultivariateSeries

using LinearAlgebra
using MultivariatePolynomials
using DynamicPolynomials

include("polynomials.jl")
include("series.jl")
include("moments.jl")
include("hankel.jl")
include("newton.jl")
include("diagonalisation.jl")
include("decompose.jl")

export Seq, seq

mutable struct Seq{T} val::T end

function seq(args...)
    if length(args)>1
        Seq([args...])
    else
        Seq(args[1])
    end
end

function DynamicPolynomials.MonomialVector(V::Vector{PolyVar{true}}, rg::Seq)
     L = DynamicPolynomials.Monomial{true}[]
     for i in rg.val
         append!(L, DynamicPolynomials.monomials(V,i))
     end
     L
end

function DynamicPolynomials.MonomialVector(V::Vector{PolyVar{true}}, rg::UnitRange{Int64})
     L = DynamicPolynomials.Monomial{true}[]
     for i in rg
         append!(L, DynamicPolynomials.monomials(V,i))
     end
     L
end

end


using JSXGraph

brd = board("brd",xlim=[-15,15], ylim=[-15,15],axis=true)

a = slider("a", [[8, 7], [12, 7], [-3, 0.1, 10]])
b = slider("b", [[8, 6], [12, 6], [-1, 1, 5]])
c = slider("c", [[8, 5], [12, 5], [-10, -5, 2]])
[a,b,c] |> brd

@jsf f(x) = val(a) * x^2 + val(b) * x + val(c)

brd ++ plot(f)
brd.style="width:500px;height:500px;margin:0 auto;"
show(brd)


########################################################################
#################### Summary Tables ####################################
########################################################################

count(x) = sum(map(y->1, x))
p10(x) = quantile(x |> collect, .1)
p25(x) = quantile(x |> collect, .25)
p50(x) = quantile(x |> collect, .50)
p75(x) = quantile(x |> collect, .75)
p90(x) = quantile(x |> collect, .9)

function default_stats(detail::Bool)
    if !detail
        return ("Obs"         => count ∘ skipmissing,
                "Mean"        => mean ∘ skipmissing,
                "Std. Dev."   => std ∘ skipmissing,
                "Min"         => minimum ∘ skipmissing,
                "Max"         => maximum ∘ skipmissing)
    else
        return ("Obs"         => count ∘ skipmissing,
                "Mean"        => mean ∘ skipmissing,
                "Std. Dev."   => std ∘ skipmissing,
                "Min"         => minimum ∘ skipmissing,
                "p10"         => p10 ∘ skipmissing,
                "p25"         => p25 ∘ skipmissing,
                "p50"         => p50 ∘ skipmissing,
                "p75"         => p75 ∘ skipmissing,
                "p90"         => p90 ∘ skipmissing,
                "Max"         => maximum ∘ skipmissing)
    end
end

NumericCol = AbstractVector{T} where {T1<:Real, T2<:Real,
                                      T<:Union{T1, Union{T2, Missing}}}

tuplefy(x) = tuple(x)
tuplefy(x::Tuple) = x

function promotearray(x::AbstractArray{S, N}) where {S,N}
    types = typeof.(x) |> unique
    T     = promote_type(types...)
    return Array{T,N}
end

function summarize(df::AbstractDataFrame, fields=names(df);
                   detail=false, stats=default_stats(detail), kwargs...)

    cols = TableCol[]
    for pair in tuplefy(stats)
        col = TableCol(pair.first)
        for header in fields
            if promotearray(df[header]) <: NumericCol
                col[header] = pair.second(df[header])
            else
                col[header] = ""
            end
        end
        push!(cols, col)
    end

    return hcat(cols...)
end

function summarize(df::AbstractDataFrame, field::Symbol; kwargs...)
    summarize(df, vcat(field); kwargs...)
end

function summarize_by(df, byname::Symbol,
                      fields=setdiff(names(df), vcat(byname));
                      kwargs...)
    tabs = []
    gd = groupby(df, byname)
    for sub in gd
        tab = summarize(sub, fields; kwargs...)
        vals= unique(sub[byname])
        length(vals) == 1 || throw(error("Groupby isn't working"))
        idx = vals[1]
        push!(tabs, idx=>tab)
    end

    return append_table(tabs...)
end

########################################################################
#################### Cross Tabulations #################################
########################################################################

function tabulate(df::AbstractDataFrame, field)

    # Count the number of observations by `field`
    tab = by(df, field) do d
        return DataFrame(N=count(d[field]))
    end

    # Construct a Frequency Column
    sort!(tab, field)
    vals  = tab[field] .|> Symbol
    freq  = tab[:N]
    pct   = freq/sum(freq)*100
    cum   = cumsum(pct)

    # Construct Table
    col1 = append_table(TableCol("Freq.",    vals, freq),
                        TableCol("Freq.", "Total"=>sum(freq)))
    col2 = append_table(TableCol("Percent",  vals, pct ),
                        TableCol("Percent", "Total"=>sum(pct)))
    col3 = TableCol("Cum.",     vals, cum )
    col = hcat(col1, col2, col3)
end


"""
    AbstractSchedule

All schedules should inherit from this type or its subtypes.
Concrete subtypes are expected to conform to the
[iteration interface](https://docs.julialang.org/en/v1/manual/interfaces/#man-interface-iteration)
and implement [`Base.getindex`](https://docs.julialang.org/en/v1/manual/interfaces/#Indexing).
"""
abstract type AbstractSchedule end

Base.firstindex(schedule::AbstractSchedule) = 1

"""
    ScheduleIterator{T<:AbstractSchedule, S}
    ScheduleIterator(schedule::T)

Create a stateful iterator around `schedule`.
See also [`next!`](#)
"""
mutable struct ScheduleIterator{T<:AbstractSchedule, S}
    schedule::T
    state::Union{S, Nothing}
end
function ScheduleIterator(schedule::T) where T<:AbstractSchedule
    _, state = iterate(schedule)

    ScheduleIterator{T, typeof(state)}(schedule, nothing)
end

"""
    next!(iter::ScheduleIterator)

Advance `iter` by one iteration and return the next value.
See also [`ScheduleIterator`](#)
"""
function next!(iter::ScheduleIterator)
    val, iter.state = isnothing(iter.state) ? iterate(iter.schedule) : iterate(iter.schedule, iter.state)

    return val
end

"""
    DecaySchedule <: AbstractSchedule

An abstract type for all decay schedules.
Such schedules conform to a formula:
```
s(t) = λ * g(t)
```
where `s(t)` is the schedule output, `λ` is the base value, and `g(t)` is the decay function.
Concrete subtypes must implment [`basevalue`](#) and [`decay`](#).
"""
abstract type DecaySchedule <: AbstractSchedule end

# decay interface
"""
    basevalue(s::T) where T<:DecaySchedule

Concrete subtypes of [`DecaySchedule`](#) must implement this function for `T`.
Given `s`, the function should return the base value of `s`.
"""
function basevalue end
"""
    decay(s::T, t) where T<:DecaySchedule

Concrete subtypes of [`DecaySchedule`](#) must implement this function for `T`.
Given `s`, `decay(s, t)` should return the value of the decay function at iteration `t`.
"""
function decay end

Base.getindex(schedule::DecaySchedule, t::Integer) = basevalue(schedule) * decay(schedule, t)

Base.iterate(schedule::DecaySchedule, t = 1) = (schedule[t], t + 1)

"""
    CyclicSchedule <: AbstractSchedule

An abstract type for all cyclic schedules.
Such schedules conform to a formula:
```
s(t) = abs(λ0 - λ1) * g(t) + min(λ0, λ1)
```
where `s(t)` is the schedule output, `λ0` is the start value,
`λ1` is the end value, and `g(t)` is the cycle function.
Concrete subtypes must implment [`startvalue`](#), [`endvalue`](#) and [`cycle`](#).
"""
abstract type CyclicSchedule <: AbstractSchedule end

# cyclic interface
"""
    startvalue(s::T) where T<:CyclicSchedule

Concrete subtypes of [`CyclicSchedule`](#) must implement this function for `T`.
Given `s`, the function should return the start value of `s`.
"""
function startvalue end
"""
    endvalue(s::T) where T<:CyclicSchedule

Concrete subtypes of [`CyclicSchedule`](#) must implement this function for `T`.
Given `s`, the function should return the end value of `s`.
"""
function endvalue end
"""
    cycle(s::T, t) where T<:CyclicSchedule

Concrete subtypes of [`CyclicSchedule`](#) must implement this function for `T`.
Given `s`, `cycle(s, t)` should return the value of the cycle function at iteration `t`.
"""
function cycle end

function Base.getindex(schedule::CyclicSchedule, t::Integer)
    k0, k1 = startvalue(schedule), endvalue(schedule)

    return abs(k0 - k1) * cycle(schedule, t) + min(k0, k1)
end

Base.iterate(schedule::CyclicSchedule, t = 1) = (schedule[t], t + 1)

"""
    Lambda{T} <: AbstractSchedule
    Lambda(;f)

Wrap an arbitrary function `f` into a schedule.
The schedule output at iteration `t` is `f(t)`.
"""
struct Lambda{T} <: AbstractSchedule
    f::T
end
Lambda(;f) = Lambda(f)

Base.getindex(schedule::Lambda, t) = schedule.f(t)

Base.iterate(schedule::Lambda, t = 1) = (schedule[t], t + 1)
Base.IteratorEltype(::Type{<:Lambda}) = Base.EltypeUnknown()
Base.IteratorSize(::Type{<:Lambda}) = Base.SizeUnkown()

"""
    reverse(f, period)

Return a reverse function such that `reverse(f, period)(t) == f(period - t)`.
"""
reverse(f, period) = t -> f(period - t)
"""
    symmetric(f, period)

Return a symmetric function such that for `t ∈ [1, period / 2)`,
the symmetric function evaluates to `f(t)`, and when `t ∈ [period / 2, period)`,
the symmetric functions evaluates to `f(period - t)`.
"""
symmetric(f, period) = t -> (t < period / 2) ? f(t) : f(period - t)

at(x::Number, t) = x
at(x::AbstractSchedule, t) = x[t]

"""
    Sequence{T<:AbstractVector, S<:Integer} <: AbstractSchedule
    Sequence(;schedules, step_sizes)

A sequence of schedules.
The output of this schedule is the concatenation of `schedules` where each
schedule is evaluated for each step size in `step_sizes`.

Note that `schedules` can also be a vector of numbers (not just schedules).

# Arguments
- `schedules::AbstractVector`: a vector of schedules or numbers
- `step_sizes::Vector{<:Integer}`: a vector of iteration lengths for each schedule
"""
struct Sequence{T<:AbstractVector, S<:Integer} <: AbstractSchedule
    schedules::T
    step_sizes::Vector{S}
end
Sequence(;schedules, step_sizes) = Sequence(schedules, step_sizes)

function Base.getindex(schedule::Sequence, t::Integer)
    accum_steps = cumsum(schedule.step_sizes)
    i = findlast(x -> t > x, accum_steps)
    i = isnothing(i) ? 1 :
            (i >= length(schedule.schedules)) ? length(schedule.schedules) : i + 1
    toffset = (i > 1) ? t - accum_steps[i - 1] : t

    return at(schedule.schedules[i], toffset)
end
function Base.iterate(schedule::Sequence, state = (1, 1, 1))
    t, i, t0 = state
    if (i <= length(schedule.step_sizes)) && (t >= t0 + schedule.step_sizes[i])
        # move onto next step range
        i += 1
        t0 = t
    end

    return at(schedule.schedules[i], t - t0 + 1), (t + 1, i, t0)
end
Base.IteratorSize(::Type{<:Sequence}) = Base.SizeUnknown()

"""
    Loop{T<:AbstractSchedule, S<:Integer} <: AbstractSchedule
    Loop(;f, period)

Create a schedule that loops `f` every `period` iterations.
Note that `f` must be a subtype of [`AbstractSchedule`](#).
To loop arbitrary functions, wrap them in [`Lambda`](#).

# Arguments
- `f::AbstractSchedule`: the schedule to loop
- `period::Integer`: how often to loop
"""
struct Loop{T<:AbstractSchedule, S<:Integer} <: AbstractSchedule
    cycle_func::T
    period::S
end
Loop(;f, period) = Loop(f, period)

function Base.getindex(schedule::Loop, t)
    tcycle = mod(t, schedule.period)
    tcycle = (tcycle == 0) ? schedule.period : tcycle

    return schedule.cycle_func[tcycle]
end

Base.iterate(schedule::Loop, t = 1) = (schedule[t], t + 1)

Base.eltype(::Type{<:Loop{T}}) where T<:Union{<:DecaySchedule, <:CyclicSchedule} = eltype(T)
Base.IteratorSize(::Type{<:Loop}) = Base.IsInfinite()


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

struct FluxMap

    Z1 :: Matrix{Complex{Float64}}
    Z2 :: Matrix{Complex{Float64}}
    dS1 :: Matrix{Float64}
    dS2 :: Matrix{Float64}
    r0 :: Float64
    dr :: Float64
    dt :: Float64
    deltaPhi :: Float64
end

function FluxMap(b::Function, r0, Nr, Nt, deltaPhi, ds)

    rs = LinRange(r0, 1, Nr)
    ts = LinRange(-pi, pi, Nt+1)[1:Nt]

    chnl = RemoteChannel(()->Channel{Bool}(), 1)
    p = Progress(Nr*Nt, desc="Tracing fieldlines... ")
    @async while take!(chnl)
        next!(p)
    end

    zs = pmap(1:Nr) do i
        col = zeros(Complex{Float64}, (4, Nt))
        for j=1:Nt
            x1 = rs[i]*[cos(ts[j]), sin(ts[j]), 0]
            x2 = x1[:]
            ds1 = 0.0
            ds2 = 0.0

            # trace forwards
            while abs(x1[3]) < deltaPhi
                x1 = rk4_step(x1, b, ds)
                ds1 += ds
            end

            # trace backwards
            while abs(x2[3]) < deltaPhi
                x2 = rk4_step(x2, b, -ds)
                ds2 += ds
            end

            # write final positions as complex numbers
            z1 = x1[1] + im*x1[2]
            z2 = x2[1] + im*x2[2]

            put!(chnl, true)
            col[:,j] = [z1, z2, ds1, ds2]
        end
        col
    end
    Z1 = vcat([c[1:1,:] for c in zs]...)
    Z2 = vcat([c[2:2,:] for c in zs]...)
    dS1 = vcat([c[3:3,:] for c in zs]...)
    dS2 = vcat([c[4:4,:] for c in zs]...)

    # make periodic
    Z1 = hcat(Z1, Z1[:,1])
    Z2 = hcat(Z2, Z2[:,1])
    dS1 = hcat(dS1, dS1[:,1])
    dS2 = hcat(dS2, dS2[:,1])

    return FluxMap(Z1, Z2, dS1, dS2, r0, rs[2]-rs[1], ts[2]-ts[1], deltaPhi)
end

function (fm::FluxMap)(z::Complex{Float64})

    r, t = abs(z), angle(z)
    i = Int64(floor((r - fm.r0) / fm.dr)) + 1
    j = Int64(floor(((t + pi) / fm.dt))) + 1
    u = (r - fm.r0) / fm.dr - i + 1
    v = (t + pi) / fm.dt - j + 1

    if (i > size(fm.Z1)[1]-1) || (j > size(fm.Z2)[2]-1) || (i < 1)
        return z, z, fm.deltaPhi, fm.deltaPhi
    end

    # bilinear interpolation
    jmax = (j==size(fm.Z1)[2]) ? 1 : j + 1
    z1 = dot([1-u, u], fm.Z1[i:i+1,[j, jmax]] * [1 - v, v])
    z2 = dot([1-u, u], fm.Z2[i:i+1,[j, jmax]] * [1 - v, v])
    ds1 = dot([1-u, u], fm.dS1[i:i+1,[j, jmax]] * [1 - v, v])
    ds2 = dot([1-u, u], fm.dS2[i:i+1,[j, jmax]] * [1 - v, v])
    return z1, z2, ds1, ds2
end

# function plot_arrows(fm::FluxMap, Nr::Int64, Nt::Int64)
#
#     rs = LinRange(fm.r0, 0.8, Nr)
#     ts = LinRange(-pi, pi, Nt+1)[1:Nt]
#
#     p = plot()
#     for r in rs
#         for t in ts
#             z1 = r*exp(im*t)
#             z2, z0 = fm(z1)[1:2]
#             plot!(p, [z0, z1, z2], arrow=true, legend=false, color=:black, linewidth=0.2)
#         end
#     end
#
#     return p
# end