text:
module CrossSection

using Statistics
using LinearAlgebra
using CSV
using DataFrames
using TriangleMesh

using ..Geometry

export AISC, wshape_nodes,
       assemble, Feature, Deck, surface_normals, avg_node_normals, xycoords_along_normal, create_CUFSM_node_elem, feature_geometry, get_xy_coordinates, area_from_cells, centroid_from_cells, moment_of_inertia_from_cells,
       triangular_mesh_properties, mesh, SectionProperties, rectangular_tube_geometry, define_w_shape_centerline_model, discretize_w_shape_centerline_model

struct WShape

    d::Float64
    tw::Float64
    bf::Float64
    tf::Float64
    kdes::Float64
    k1::Float64
    A::Float64
    Ix::Float64
    Iy::Float64
    J::Float64
    Cw::Float64
    Zx::Float64
    Zy::Float64
    Wno::Float64

end

struct CrossSectionBranch

    anchor::Tuple{Float64, Float64}
    direction::Float64
    magnitude::Float64
    n::Int64

end

#primitive, line element defined as vector, with n segments
struct Feature

    ΔL::Array{Float64,1}
    θ::Array{Float64,1}
    n::Array{Int,1}
    radius::Array{Float64,1}
    n_radius::Array{Int,1}
    closed_or_open::Int

end

struct Point

    x::Float64
    y::Float64

end

#deck cross-section definition
#this could be a model for other cross-section types
struct Deck

    features::Tuple{Feature,Feature,Feature}
    feature_map::Array{Int,1}

end

struct Open

    features::Feature
    feature_map::Array{Int,1}

end

#Get the node and element properties just for the flange and lip of a C or Z section.
#This code grabs the bottom flange and lip.
function CZflange_template(CorZ,H,Bc,Bt,Dc,Dt,r1,r2,r3,r4,θc,θt,t,nh,nb1,nb2,nd1,nd2,nr1,nr2,nr3,nr4,kipin,center)

    prop,node,elem,lengths,springs,constraints,geom,cz = CrossSection.CUFSMtemplate(CorZ,H,Bc,Bt,Dc,Dt,r1,r2,r3,r4,θc,θt,t,nh,nb1,nb2,nd1,nd2,nr1,nr2,nr3,nr4,kipin,center)

    if CorZ == 2
        node[:, 2] = -node[:, 2]
    end

    index_xo = findall(x-> x==0.0, node[:, 2])
    index_yo = findall(y-> y==0.0, node[:, 3])
    index_o = intersect(index_xo, index_yo)
    index = 1:(index_o[1]+1)

	nodeflange = node[index,:]
    elemflange = elem[index[1:end-1],:]

    return nodeflange, elemflange

end

"""
    AISC(shape_name)

Accepts `shape_name` as a String and returns cross-section dimensions and section properties in the Struct `shape_info`.

For now only W-shapes have been tested, eg., `AISC("W14x90")` where the struct contains `d, tw, bf, tf, kdes, k1, A, Ix, Iy, J, Cw, Zx, Wno`.

"""

function AISC(shape_name)

    filename = string(@__DIR__, "/assets/aisc-shapes-database-v15.0.csv")

    data = CSV.File(filename)

    shape_row = findfirst(==(shape_name), data.AISC_Manual_Label)

    section_type = data.Type[shape_row]

    if section_type == "W"

        d = parse(Float64, data.d[shape_row])
        tw = parse(Float64, data.tw[shape_row])
        bf = parse(Float64, data.bf[shape_row])
        tf = parse(Float64, data.tf[shape_row])
        kdes = parse(Float64, data.kdes[shape_row])

        #get k1 from AISC table, it is in fraction format
        k1 = data.k1[shape_row]
        index = findfirst("/", k1)

        if length(k1) == 7
            whole = Int(data.k1[shape_row][1] - '0')
        else
            whole = 0.0
        end

        if isempty(index) == false
            top_fraction = parse(Float64, data.k1[shape_row][index[1]-2:index[1]-1])
            bottom_fraction = parse(Float64, data.k1[shape_row][index[1]+1:index[1]+2])
        else
            top_fraction = 0.0
            bottom_fraction = 0.0
        end

        k1 = whole + top_fraction/bottom_fraction

        A = data.A[shape_row]
        Ix = data.Ix[shape_row]
        Iy = data.Iy[shape_row]
        J = parse(Float64, data.J[shape_row])
        Cw = parse(Float64, data.Cw[shape_row])
        Zx = data.Zx[shape_row]
        Zy = data.Zy[shape_row]
        Wno = parse(Float64, data.Wno[shape_row])

        shape_info = WShape(d, tw, bf, tf, kdes,k1, A, Ix, Iy, J, Cw, Zx, Zy, Wno)

    end

    return shape_info

end

"""
    wshape_nodes(shape_info, n)

Accepts the Struct `shape_info` generated using CrossSection.AISC and the discretization Vector `n` and outputs the outline x-y coordinates of a W shape 'xcoords' and 'ycoords'.

The Vector 'n' describes the number of segments in a quarter cross-section, i.e., `n = [half of outside flange face, flange thickness, half of inside flange face, flange-web radius, half of web]`.

"""

function wshape_nodes(shape_info, n)

    #from bottom of bottom flange, web centerline to left edge
    xcoords = zeros(n[1]+1)
    ycoords = zeros(n[1]+1)

    flange_range = 0.0 : -shape_info.bf / 2 / n[1] : -shape_info.bf / 2
    [xcoords[i] =  flange_range[i] for i in eachindex(flange_range)]
    ycoords .= 0.0

    #up along bottom flange thickness
    flange_thickness_range = shape_info.tf/n[2]:shape_info.tf/n[2]:shape_info.tf
    xcoords = [xcoords; ones(n[2])*xcoords[end]]
    ycoords = [ycoords; flange_thickness_range]

    #over to fillet radius at bottom flange - web intersection

    # flange_flat = shape_info.bf/2 - shape_info.k1
    flange_flat = shape_info.bf/2 - shape_info.tw/2 - (shape_info.kdes - shape_info.tf)

    inside_flange_range = (xcoords[end] + flange_flat/n[3]) : flange_flat/n[3] : (xcoords[end] + flange_flat)

    xcoords = [xcoords; inside_flange_range]
    ycoords = [ycoords; ones(n[3])*ycoords[end]]

    #go around the fillet
    radius = -xcoords[end] - shape_info.tw/2
    θ = (-π/2 + π/2/n[4]):π/2/n[4]: 0.0

    xo = xcoords[end]
    yo = ycoords[end] + radius

    x_radius = xo .+ radius .* cos.(θ)
    y_radius = yo .+ radius .* sin.(θ)

    # plot(x_radius, y_radius, markershape = :o, linetype = :scatter)

    xcoords = [xcoords; x_radius]
    ycoords = [ycoords; y_radius]

    #add web flat
    web_flat = shape_info.d/2 - shape_info.tf - radius

    web_flat_range = LinRange(ycoords[end] + web_flat/n[5], (ycoords[end] + web_flat), n[5])
    # web_flat_range = (ycoords[end] + web_flat/n[5]): web_flat/n[5]: (ycoords[end] + web_flat)
    xcoords = [xcoords; ones(n[5])*xcoords[end]]
    ycoords = [ycoords; web_flat_range]

    #mirror about horizontal axis
    ycoords_horz_flip = ycoords .- ycoords[end]
    ycoords_horz_flip = -ycoords_horz_flip
    ycoords_horz_flip = ycoords_horz_flip .+ ycoords[end]

    xcoords = [xcoords; reverse(xcoords)[2:end]]
    ycoords = [ycoords; reverse(ycoords_horz_flip)[2:end]]

    #mirror about vertical axis
    xcoords_vert_flip = reverse(-xcoords)[2:end-1]

    xcoords = [xcoords; xcoords_vert_flip]
    ycoords = [ycoords; reverse(ycoords)[2:end-1]]

    return xcoords, ycoords

end

# #define nodal coordinates within a feature
# function discretize_feature(feature)

#     dx = feature.Δx ./ feature.n
#     dy = feature.Δy ./ feature.n

#     return dx, dy

# end

#define feature geometry, typically a feature is repetitive
function feature_geometry(feature, dx, dy)

    xcoords = []
    ycoords = []

    num_lines = length(feature.Δx)

    for i = 1:num_lines

        if i==1
            xcoords = range(0.0, feature.Δx[i], length = feature.n[i] + 1)
            ycoords = range(0.0, feature.Δy[i], length = feature.n[i] + 1)
        else
            xcoords = [xcoords; xcoords[end] .+ range(dx[i], feature.Δx[i], length = feature.n[i])]
            ycoords = [ycoords; ycoords[end] .+ range(dy[i], feature.Δy[i], length = feature.n[i])]
        end

    end

    return xcoords, ycoords

end

#assemble an open cross-section as a series of line element features
function assemble(OpenSection)

    xcoords = []
    ycoords = []
    num_features = length(OpenSection.feature_map)

    for i =1:num_features

        feature_type = OpenSection.feature_map[i]

        # dx, dy = discretize_feature(OpenSection.features[feature_type])

        if i==1
            xcoords, ycoords = get_xy_coordinates(OpenSection.features[feature_type])
        else
            feature_xcoords, feature_ycoords = get_xy_coordinates(OpenSection.features[feature_type])
            xcoords = [xcoords; xcoords[end] .+ feature_xcoords[2:end]]
            ycoords = [ycoords; ycoords[end] .+ feature_ycoords[2:end]]
        end

    end

    return xcoords, ycoords

end

# calculate surface normals for each line segment in a 2D cross-section
function surface_normals(xcoords, ycoords, closed_or_open)

    if closed_or_open == 0
        numel = length(xcoords)   #closed
    elseif closed_or_open ==1
        numel = length(xcoords) - 1  #open
    end

    unitnormals = zeros(Float64, (numel, 2))

    for i=1:numel

        if (i == numel) & (closed_or_open == 0)   #for tubes
            pointA = Point(xcoords[i], ycoords[i])
            pointB = Point(xcoords[1], ycoords[1])
        else
            pointA = Point(xcoords[i], ycoords[i])
            pointB = Point(xcoords[i + 1], ycoords[i + 1])
        end

        dx = pointB.x - pointA.x
        dy = pointB.y - pointA.y

        normAB = norm([dx, dy])

        unitnormals[i, :] = [-dy, dx] / normAB

        if unitnormals[i,1] == -0.0
            unitnormals[i,1]= 0.0
        end

        if unitnormals[i,2] == -0.0
            unitnormals[i,2]= 0.0
        end

    end

    return unitnormals

end

# calculate average unit normals at each node in a 2D cross-section from element unit normals
function avg_node_normals(unitnormals, closed_or_open)

    if closed_or_open == 0
        numnodes = size(unitnormals)[1]
        numel = size(unitnormals)[1]
    elseif closed_or_open ==1
        numnodes = size(unitnormals)[1]+1
        numel = size(unitnormals)[1]
    end

    nodenormals = zeros(Float64, (numnodes, 2))

    for i=1:numnodes

        if (i == 1) & (closed_or_open == 0)  # where nodes meet in the tube
            nodenormals[i, :] = mean(unitnormals[[numel, 1], :], dims=1)
        elseif (i != 1) & (closed_or_open == 0)  #tube
            nodenormals[i, :] = mean(unitnormals[i-1:i, :], dims=1)
        elseif (i == 1) & (closed_or_open == 1)  #open, first node is element norm
            nodenormals[i, :] = unitnormals[i, :]
        elseif (i != 1) & (i != numnodes) & (closed_or_open == 1)  #open
            nodenormals[i, :] = mean(unitnormals[i-1:i, :], dims=1)
        elseif (i == numnodes) & (closed_or_open == 1)  #open, last node is element norm
            nodenormals[i, :] = unitnormals[i-1, :]

        end

        #make sure unit normal always = 1.0
        unitnorm = norm(nodenormals[i, :])
        if unitnorm < 0.99
            scale = 1.0/unitnorm
            nodenormals[i,:] = scale .* nodenormals[i,:]
        end

    end

    return nodenormals

end

function xycoords_along_normal(xcoords, ycoords, nodenormals, Δ)

    numnodes = size(xcoords)[1]
    xcoords_normal = zeros(Float64, numnodes)
    ycoords_normal = zeros(Float64, numnodes)

    for i=1:numnodes
        xcoords_normal[i] = xcoords[i] + nodenormals[i, 1] * Δ
        ycoords_normal[i] = ycoords[i] + nodenormals[i, 2] * Δ
    end

    return xcoords_normal, ycoords_normal

end

function create_CUFSM_node_elem(xcoords, ycoords, connectivity, t)

    num_elem = size(connectivity)[1]
    num_nodes = length(xcoords)

    node = zeros((num_nodes, 8))
    elem = zeros((num_elem, 5))

    for i=1:num_nodes
        node[i,:] = [i, xcoords[i], ycoords[i], 1, 1, 1, 1, 1.0]
    end

    for i=1:num_elem
        elem[i, :] = [i, connectivity[i,1], connectivity[i,2], t[i], 100]
    end

    return node, elem

end

function get_xy_coordinates(feature)

    xcoords = []
    ycoords = []

    #convert feature vectors to xy components
    Δxy = Geometry.vector_components(feature.ΔL, feature.θ)

    #number of straight line segments in the feature
    num_lines = size(Δxy)[1]

    #get xy line coordinates of feature
    xcoords_straight, ycoords_straight = Geometry.line_coordinates(Δxy, feature.closed_or_open)

    #calculate feature surface normals
    unitnormals = CrossSection.surface_normals(xcoords_straight, ycoords_straight, feature.closed_or_open)

    #calculate feature node normals
    nodenormals = CrossSection.avg_node_normals(unitnormals, feature.closed_or_open)

    #calculate interior corner angle magnitudes
    num_radius = length(feature.radius)
    interior_radius_angle = zeros(Float64, num_radius)
    for i=1:num_radius

        θ1 = feature.θ[i]
        θ2 = feature.θ[i+1]

        if sign(θ1) != sign(θ2)   #don't think this is fully general
            interior_radius_angle[i] = 180 - (abs(θ1) + abs(θ2))
        end

    end

    #calculate distance from curve PI to start and end of curve, along tangents
    Tc = zeros(Float64, length(feature.radius))

    for i = 1:length(feature.radius)

        radius = feature.radius[i]

        if radius > 0.0

            xy_PI = [xcoords_straight[i+1], ycoords_straight[i+1]]
            n = feature.n_radius[i]
            γ = interior_radius_angle[i]
            θ1 = feature.θ[i]
            θ2 = feature.θ[i+1]

            if (sign(θ1) <=0) & (sign(θ2) >= 0)
                PI_unit_normal = -nodenormals[i+1,:]
            else
                PI_unit_normal = nodenormals[i+1,:]
            end

            xy_curve, Δ, E, Tc[i], xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

        elseif radius == 0.0
            Tc[i] = 0.0
        end

    end

    #shorten vector lengths to include curves
    ΔLc = zeros(Float64, num_lines)
    for i = 1:length(feature.ΔL)

        if i == 1  #first
            ΔLc[i] = feature.ΔL[i] - Tc[i]   #j end
        elseif i == length(feature.ΔL)  #last
            ΔLc[i] = feature.ΔL[i] - Tc[end]  #j end
        else  #others
            ΔLc[i] = feature.ΔL[i] - Tc[i-1] - Tc[i] #i and j ends
        end

    end

    Δxyc = Geometry.vector_components(ΔLc, feature.θ)

    #discretize feature
    dx = Δxyc[:,1] ./ feature.n
    dy = Δxyc[:,2] ./ feature.n

    #assemble feature
    for i = 1:num_lines

        if i==1   #first vector
            xcoords = range(0.0, Δxyc[i,1], length = feature.n[i] + 1)
            ycoords = range(0.0, Δxyc[i,2], length = feature.n[i] + 1)
        elseif i != num_lines
            xcoords = [xcoords; xcoords[end] .+ range(dx[i], Δxyc[i,1], length = feature.n[i])]
            ycoords = [ycoords; ycoords[end] .+ range(dy[i], Δxyc[i,2], length = feature.n[i])]
        end

        #add radius to end of vector
        if i < num_lines
            if feature.radius[i]>0.0
                xy_PI = [xcoords_straight[i+1], ycoords_straight[i+1]]
                n = feature.n_radius[i]
                radius = feature.radius[i]
                γ = interior_radius_angle[i]
                θ1 = feature.θ[i]
                θ2 = feature.θ[i+1]

                if (sign(θ1) <=0) & (sign(θ2) >= 0)
                    PI_unit_normal = -nodenormals[i+1,:]  #concave up
                else
                    PI_unit_normal = nodenormals[i+1,:]  #concave down
                end

                xy_curve, Δ, E, T, xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

                if (sign(θ1) <=0) & (sign(θ2) >= 0)
                    xy_curve = reverse(xy_curve, dims=1)    #concave up
                end

                xcoords = [xcoords; xy_curve[2:end,1]]
                ycoords = [ycoords; xy_curve[2:end,2]]

            end
        end

        if i== num_lines  #last vector
            xcoords = [xcoords; xcoords[end] .+ range(dx[i], Δxyc[i,1], length = feature.n[i])]
            ycoords = [ycoords; ycoords[end] .+ range(dy[i], Δxyc[i,2], length = feature.n[i])]
        end

    end

    return xcoords, ycoords

end

#calculate cross-sectional area from cells

function area_from_cells(Ai)

    A = sum(Ai)

end

#calculate cross-section centroid from cells

function centroid_from_cells(Ai, cxi, cyi)

    A = area_from_cells(Ai)

    cx = sum(cxi .* Ai) / A
    cy = sum(cyi .* Ai) / A

    return cx, cy

end

function moment_of_inertia_from_cells(Ai, ci, c)

    I = sum(((c .- ci) .^2 .* Ai))

    return I

end

#discretize cross-section with a triangular mesh
function triangular_mesh(xcoords, ycoords, mesh_size)

    num_nodes = length(xcoords)
    num_segments = num_nodes

    # n_point, n_point_marker, n_point_attribute, n_segment, n_holes
    poly = TriangleMesh.Polygon_pslg(num_nodes, 1, 0, num_segments, 0)

    node = [xcoords ycoords]
    set_polygon_point!(poly, node)

    node_marker = ones(Int, num_nodes, 1)
    set_polygon_point_marker!(poly, node_marker)

    segments = zeros(Int, num_segments, 2)
    for i=1:num_segments

        if i == num_segments
            segments[i, 1:2] = [i, 1]
        else
            segments[i, 1:2] = [i, i+1]
        end

    end

    set_polygon_segment!(poly, segments)

    segment_markers = ones(Int, num_segments)
    set_polygon_segment_marker!(poly, segment_markers)

    #switches from https://www.cs.cmu.edu/~quake/triangle.html
    switches = "penvVa" * string(mesh_size) * "D"

    mesh = create_mesh(poly, switches)

    return mesh

end

function triangular_mesh_properties(mesh)

    #calculate cell area and centroid
    Ai = zeros(Float64, mesh.n_cell)
    cxi = zeros(Float64, mesh.n_cell)
    cyi = zeros(Float64, mesh.n_cell)

    for i = 1:mesh.n_cell

        p1 = mesh.cell[1, i]
        p2 = mesh.cell[2, i]
        p3 = mesh.cell[3, i]

        x1 = mesh.point[1, p1]
        y1 = mesh.point[2,p1]
        x2 = mesh.point[1, p2]
        y2 = mesh.point[2,p2]
        x3 = mesh.point[1, p3]
        y3 = mesh.point[2,p3]

    Ai[i] = Geometry.triangle_area(x1, y1, x2, y2, x3, y3)

    cxi[i], cyi[i] = Geometry.triangle_centroid(x1, y1, x2, y2, x3, y3)

    end

    return Ai, cxi, cyi

end

function mesh(xcoords, ycoords, mesh_size)

    section_mesh = CrossSection.triangular_mesh(xcoords, ycoords, mesh_size)
    Ai, cxi, cyi = CrossSection.triangular_mesh_properties(section_mesh)

    return Ai, cxi, cyi

 end

 function rectangular_tube_geometry(feature)

    xcoords = []
    ycoords = []

    #convert feature vectors to xy components
    Δxy = Geometry.vector_components(feature.ΔL, feature.θ)

    #number of straight line segments in the feature
    num_lines = size(Δxy)[1]

    #get xy line coordinates of feature
    xcoords_straight, ycoords_straight = Geometry.line_coordinates(Δxy, feature.closed_or_open)

    #calculate feature surface normals
    unitnormals = CrossSection.surface_normals(xcoords_straight, ycoords_straight, feature.closed_or_open)

    #calculate feature node normals
    nodenormals = CrossSection.avg_node_normals(unitnormals, feature.closed_or_open)

    #calculate interior corner angle magnitudes
    num_radius = length(feature.radius)
    interior_radius_angle = zeros(Float64, num_radius)

    for i=1:num_radius

        θ1 = feature.θ[i] - 180

        if (feature.closed_or_open == 0) & (i==num_radius)  #for closed section, return to beginning node
            θ2 = feature.θ[1]
        else
            θ2 = feature.θ[i+1]
        end

        unit_vector_i = [cos(deg2rad(θ1)), sin(deg2rad(θ1))]
        unit_vector_j = [cos(deg2rad(θ2)), sin(deg2rad(θ2))]

        interior_radius_angle[i] = rad2deg(acos(dot(unit_vector_i, unit_vector_j)))

    end

    #calculate distance from curve PI to start and end of curve, along tangents
    Tc = zeros(Float64, length(feature.radius))

    for i = 1:length(feature.radius)

        radius = feature.radius[i]

        if radius > 0.0

            if feature.closed_or_open == 0
                xy_PI = [xcoords_straight[i], ycoords_straight[i]]
            elseif feature.closed_or_open == 1
                xy_PI = [xcoords_straight[i+1], ycoords_straight[i+1]]
            end

            n = feature.n_radius[i]
            γ = interior_radius_angle[i]

            θ1 = feature.θ[i]

            if (feature.closed_or_open == 0) & (i == length(feature.radius))  #for closed section, return to beginning node
                θ2 = feature.θ[1]
            else
                θ2 = feature.θ[i+1]
            end

            if (feature.closed_or_open == 0) & (i == length(feature.radius))

                if (sign(θ1) <=0) & (sign(θ2) >= 0)
                    PI_unit_normal = -nodenormals[1,:]
                else
                    PI_unit_normal = nodenormals[1,:]
                end

            else

                if (sign(θ1) <=0) & (sign(θ2) >= 0)
                    PI_unit_normal = -nodenormals[i+1,:]
                else
                    PI_unit_normal = nodenormals[i+1,:]
                end

            end

            xy_curve, Δ, E, Tc[i], xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

        elseif radius == 0.0
            Tc[i] = 0.0
        end

    end

    #Shorten vector lengths to include curves.
    ΔLc = zeros(Float64, num_lines)

    for i = 1:length(feature.ΔL)

        if i == 1  #first
            if feature.closed_or_open == 1
                ΔLc[i] = feature.ΔL[i] - Tc[i]   #j end
            elseif feature.closed_or_open == 0
                ΔLc[i] = feature.ΔL[i] - Tc[i] - Tc[end]
            end
        elseif i == length(feature.ΔL)  #last
            if feature.closed_or_open == 1
                ΔLc[i] = feature.ΔL[i] - Tc[end]  #j end
            elseif feature.closed_or_open == 0
                ΔLc[i] = feature.ΔL[i] - Tc[end] - Tc[i - 1]
            end
        else  #others
            ΔLc[i] = feature.ΔL[i] - Tc[i-1] - Tc[i] #i and j ends
        end

    end

    #Get xy components of flat lengths around tube.
    Δxyc = Geometry.vector_components(ΔLc, feature.θ)

    #Discretize feature.
    dx = Δxyc[:,1] ./ feature.n
    dy = Δxyc[:,2] ./ feature.n

    #Define vertical tube segment at y=0.

    ycoords = Tc[1]:dy[1]:(Δxyc[1,2] + Tc[1])
	xcoords = zeros(Float64, length(ycoords))

    #Define top left corner.

    n = feature.n_radius[1]
    radius = feature.radius[1]
    γ = interior_radius_angle[1]
    PI_unit_normal = nodenormals[2,:]
    xy_PI = [xcoords_straight[2], ycoords_straight[2]]

    xy_curve, Δ, E, T, xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

    xcoords = [xcoords; xy_curve[2:end, 1]]
    ycoords = [ycoords; xy_curve[2:end, 2]]

    #Define top flat.

    x_range = xcoords[end]:dx[2]:xcoords[end]+Δxyc[2,1]
    y_range = ycoords[end] * ones(length(x_range))
    xcoords = [xcoords; x_range[2:end]]
    ycoords = [ycoords; y_range[2:end]]

    #Define top right corner.

    n = feature.n_radius[2]
    radius = feature.radius[2]
    γ = interior_radius_angle[2]
    PI_unit_normal = nodenormals[3,:]
    xy_PI = [xcoords_straight[3], ycoords_straight[3]]

    xy_curve, Δ, E, T, xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

    xcoords = [xcoords; xy_curve[2:end, 1]]
    ycoords = [ycoords; xy_curve[2:end, 2]]

    #Define right vertical flat.

    y_range = ycoords[end]:dy[3]:(ycoords[end] + Δxyc[3,2])
    x_range = xcoords[end] * ones(length(y_range))
    xcoords = [xcoords; x_range[2:end]]
    ycoords = [ycoords; y_range[2:end]]

    #Define bottom right corner.

    n = feature.n_radius[3]
    radius = feature.radius[3]
    γ = interior_radius_angle[3]
    PI_unit_normal = nodenormals[4,:]
    xy_PI = [xcoords_straight[4], ycoords_straight[4]]

    xy_curve, Δ, E, T, xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

    xcoords = [xcoords; xy_curve[2:end, 1]]
    ycoords = [ycoords; xy_curve[2:end, 2]]

    #Define bottom horizontal flat.

    x_range = xcoords[end]:dx[4]:xcoords[end]+Δxyc[4,1]
    y_range = ycoords[end] * ones(length(x_range))
    xcoords = [xcoords; x_range[2:end]]
    ycoords = [ycoords; y_range[2:end]]

    #Define lower left corner.

    n = feature.n_radius[4]
    radius = feature.radius[4]
    γ = interior_radius_angle[4]
    PI_unit_normal = nodenormals[1,:]
    xy_PI = [xcoords_straight[1], ycoords_straight[1]]

    xy_curve, Δ, E, T, xy_o, xy_BC, xy_EC, BC_unit_tangent, EC_unit_tangent, radius_unit_vector_BC = Geometry.circular_curve(radius, γ, xy_PI, PI_unit_normal, n)

    xcoords = [xcoords; xy_curve[2:(end-1), 1]]
    ycoords = [ycoords; xy_curve[2:(end-1), 2]]

    return xcoords, ycoords

end

function discretize_w_shape_centerline_model(shape, cross_section)

    num_branches = size(shape)[1]

    xcoords = []
    ycoords = []

    for i = 1:num_branches

        ΔL = shape[i].magnitude
        θ = shape[i].direction
        n = shape[i].n
        Δxy = Geometry.vector_components(ΔL, θ)
        anchor = shape[i].anchor

        if i == 1
            xcoords = range(0.0, Δxy[1], length = n + 1) .+ anchor[1]
            ycoords = range(0.0, Δxy[2], length = n + 1) .+ anchor[2]

        else
            xcoords = [xcoords; range(0.0, Δxy[1], length = n + 1) .+ anchor[1]]
            ycoords = [ycoords; range(0.0, Δxy[2], length = n + 1) .+ anchor[2]]

        end

    end

    #Round here to help unique function.
    xycoords = [(round(xcoords[i], digits = 3), round(ycoords[i], digits = 3)) for i = 1:length(xcoords)]

    xycoords = unique(xycoords)

    coord = [y[i] for y in xycoords, i in 1:2]

    #Shift coordinates so that web is centered on x=0.
    coord[:, 1] = coord[:, 1] .- cross_section.bf/2

    #Shift coordinates so that bottom fiber is at y=0.
    coord[:, 2] = coord[:, 2] .+ cross_section.tf/2

    #Define element connectivity.

    num_elem = sum([shape[i].n for i=1:num_branches])

    node_start = 1
    node_end = shape[1].n

    node_i = node_start:node_end
    node_j = node_i .+ 1

    node_start = floor(Int, shape[1].n/2)+1
    node_end = node_end + 2

    node_i = [node_i; node_start]
    node_j = [node_j; node_end]

    node_start = node_end
    node_end = node_end + shape[2].n - 2

    node_i = [node_i; node_start:node_end]
    node_j = [node_j; (node_start:node_end) .+ 1]

    node_start = shape[1].n + shape[2].n + 2
    node_end = node_start + floor(Int, shape[2].n/2) - 1
    node_i_range = range(node_start, node_end-1)
    node_j_range = node_i_range .+ 1

    node_i = [node_i; node_i_range]
    node_j = [node_j; node_j_range]

    node_start = node_i[end] + 1
    node_end = shape[1].n + shape[2].n + 1

    node_i = [node_i; node_start]
    node_j = [node_j; node_end]

    node_start = node_j[end]
    node_end = node_i[end] + 1

    node_i = [node_i; node_start]
    node_j = [node_j; node_end]

    node_start = shape[1].n + shape[2].n + 2 + floor(Int, shape[3].n/2)
    node_end = node_start + floor(Int, shape[3].n/2) - 1
    node_i_range = range(node_start, node_end-1)
    node_j_range = node_i_range .+ 1

    node_i = [node_i; node_i_range]
    node_j = [node_j; node_j_range]

    t = [ones(Float64, shape[1].n)*cross_section.tf[1]; ones(Float64, shape[2].n)*cross_section.tw[1]; ones(Float64, shape[2].n)*cross_section.tf[1]]

    ends = [node_i node_j t]

    return coord, ends

end

function define_w_shape_centerline_model(bf, tf, d, n)

    num_branches = 3
    w_shape = Vector{CrossSectionBranch}(undef, num_branches)

    #first branch, bottom flange
    anchor = (0.0, 0.0)
    direction = 0.0
    magnitude = bf

    w_shape[1] = CrossSectionBranch(anchor, direction, magnitude, n[1])

    #second branch, web
    anchor = (bf/2, 0.0)
    direction = 90.0
    magnitude = d - tf

    w_shape[2] = CrossSectionBranch(anchor, direction, magnitude, n[2])

    #third branch, top flange
    anchor = (0.0, d - tf)
    direction = 0.0
    magnitude = bf

    w_shape[3] = CrossSectionBranch(anchor, direction, magnitude, n[3])

    return w_shape

end

end #module


text:
# ---
# title: 83. Remove Duplicates from Sorted List
# id: problem83
# author: zhwang
# date: 2022-02-17
# difficulty: Easy
# categories: Linked List
# link: <https://leetcode.com/problems/remove-duplicates-from-sorted-list/description/>
# hidden: true
# ---
#
# Given a sorted linked list, delete all duplicates such that each element
# appear only _once_.
#
# **Example 1:**
#
#
#
#     Input: 1->1->2
#     Output: 1->2
#
#
# **Example 2:**
#
#
#
#     Input: 1->1->2->3->3
#     Output: 1->2->3
#
#
#
## @lc code=start
using LeetCode

function delete_duplicates!(head::Union{ListNode,Nothing})::Union{ListNode,Nothing}
    cur = head
    while !isnothing(cur)
        node, value = next(cur), val(cur)
        while !isnothing(node) && val(node) == value
            cur.next = node = next(node)
        end
        cur = next(cur)
    end
    return head
end
## @lc code=end

ext: jl


text:
function payoff(S1::abstractMatrix, amPayoff::AmericanPayoff, rfCurve::abstractZeroRateCurve, T1::num2 = maturity(amPayoff)) where {abstractZeroRateCurve <: AbstractZeroRateCurve, abstractMatrix <: AbstractMatrix{num}} where {num <: Number, num2 <: Number}
    T = amPayoff.T
    NStep = size(S1, 2) - 1
    index1 = round(Int, T / T1 * NStep) + 1
    S = collect(S1[:, 1:index1])
    S0 = first(S)
    (Nsim, Nstep) = size(S)
    Nstep -= 1
    r = rfCurve.r
    dt = T / Nstep
    # initialize vectors
    exerciseTimes = (Nstep) .* ones(Int64, Nsim)
    df_exerciseTimes = [exp.(-integral(r, dt * j_)) for j_ = 0:NStep]
    #define payout
    phi(Sti::numtype_) where {numtype_ <: Number} = payout(Sti, amPayoff)
    #compute payout
    @views V = phi.(S[:, end])
    # Backward Procedure
    @inbounds for j = Nstep:-1:2
        @views Tmp = phi.(S[:, j])
        inMoneyIndexes = findall(Tmp .> 0.0)
        if !isempty(inMoneyIndexes)
            S_I = view(S, inMoneyIndexes, j)
            #-- Intrinsic Value
            @views IV = Tmp[inMoneyIndexes]
            #-- Continuation Value
            #- Linear Regression on Quadratic Form
            A = [ones(length(S_I)) S_I S_I .^ 2]
            b = [V[j_] * df_exerciseTimes[exerciseTimes[j_]-j+1] for j_ in inMoneyIndexes]
            LuMat = lu!(A' * A)
            Btilde = A' * b
            alpha = LuMat \ Btilde
            #alpha=A\b;
            #Continuation Value
            CV = A * alpha
            #----------
            # Find premature exercise times
            Index = findall(IV .> CV)
            @views exercisePositions = inMoneyIndexes[Index]
            # Update the outputs
            @views V[exercisePositions] = IV[Index]
            @views exerciseTimes[exercisePositions] .= j - 1
        end
    end
    Out = V .* df_exerciseTimes[exerciseTimes.+1]

    return Out
end


text:
function getUserInput(T = String, msg = "")
    print("$msg ")
    if T == String
        return readline()
    else
    try
        return parse(T,readline())
    catch
        println("Sorry, I could not interpret your answer. Please try again")
        getUserInput(T,msg)
        end
    end
end

function factorize(n)
    x = String[];
    for i in 2:(n / 2)
        if n % i == 0
            n = n / i;
            resize!(x, length(x) + 1);
            x[length(x)] = string(i);
        end
    end
    return join(x, " x ");
end

println(factorize(getUserInput(Int64,"Write a number:")));

max_stars_repo_name: MediocreBoris/gitgud

max_issues_repo_name: MediocreBoris/gitgud

max_forks_repo_name: MediocreBoris/gitgud


text:
using CUDA, Crux

## Only run CUDA tests if it seems to be working properly
USE_CUDA = false
try
	cu(zeros(Float32, 10, 10))
	USE_CUDA = true
	CUDA.allowscalar(false)
catch end

## Run basic functionality tests
include("spaces_tests.jl")
USE_CUDA && include("devices_tests.jl")
include("util_tests.jl")
include("experience_buffer_tests.jl")
include("sampler_tests.jl")
include("policy_tests.jl")
include("logging_tests.jl")

## Extras tests
include("extras_tests.jl")

## Solvers tests
include("solver_tests.jl")


text:
@testset "Persistence" begin
    (f, fio) = mktemp()
    close(fio)

    gx = PathGraph(5)
    for g in testgraphs(gx)
        mg = MetaGraph(g)
        set_prop!(mg, 1, 2, :weight, 0.2)
        set_prop!(mg, 2, 3, :color, :red)
        set_prop!(mg, 1, :name, "Alice")
        set_prop!(mg, 2, :name, "Bob")
        set_prop!(mg, :type, "My MetaGraph")

        @test savegraph(f, mg) == 1
        g2 = loadgraph(f, MGFormat())
        @test mg == g2
    end

    gx = PathDiGraph(5)
    for g in testdigraphs(gx)
        mg = MetaDiGraph(g)
        set_prop!(mg, 1, 2, :weight, 0.2)
        set_prop!(mg, 2, 3, :color, :red)
        set_prop!(mg, 1, :name, "Alice")
        set_prop!(mg, 2, :name, "Bob")
        set_prop!(mg, :type, "My MetaGraph")

        @test savegraph(f, mg) == 1
        g2 = loadgraph(f, MGFormat())
        @test mg == g2
    end
    rm(f)
end


text:
# Operators in the harmonic oscillator basis.

"""
Harmonic oscillator basis for `S` surfaces and `M` modes.
"""
struct Basis{S,M}
    "Single-mode basis size."
    size::Int

    "Single-surface basis size."
    dim1::Int
    "Total basis size."
    dim::Int
end

"""
    Basis(::System, size::Int)

Create a basis with `size` basis functions for a single mode.
"""
function Basis(::System{S,M}, size::Int) where {S,M}
    size >= 1 || throw(DomainError(size, "At least 1 basis function."))

    dim1 = size^M
    dim = S * dim1

    Basis{S,M}(size, dim1, dim)
end

"""
    id(basis::Basis)

Identity operator in `basis` for a single mode.
"""
id(basis::Basis) = Matrix{Float64}(I, basis.size, basis.size)

"""
    a(basis::Basis)

Annihilation operator in `basis` for a single mode.
"""
a(basis::Basis) = diagm(1 => sqrt.(1.0:basis.size-1))

"""
    n(basis::Basis)

Number operator in `basis` for a single mode.
"""
n(basis::Basis) = diagm(0 => 0.0:basis.size-1)

"""
    q(basis::Basis)

Position operator in `basis` for a single mode.
"""
q(basis::Basis) = (a(basis)' + a(basis)) / sqrt(2.0)

"""
    mkop(basis::Basis, op::Matrix{Float64}, idx::Int)

Create a multi-mode operator in `basis` that is the tensor product of `op` at
mode `idx` and the identity operator at all other modes.
"""
function mkop(basis::Basis{S,M}, op::Matrix{Float64}, idx::Int) where {S,M}
    # Nothing to do for a single mode.
    M == 1 && return op

    ops = []
    for m in 1:M
        if m == idx
            push!(ops, op)
        else
            push!(ops, id(basis))
        end
    end

    result = kron(ops...)
    size(result) == (basis.dim1, basis.dim1) || @warn "Bad size in mkop"

    result
end

"""
    mkop(basis::Basis, op, idx::Int)

Create a multi-mode operator in `basis` that is the tensor product of `op` at
mode `idx` and the identity operator at all other modes, where `op` is a
function that generates the operator in the basis.
"""
mkop(basis::Basis, op, idx::Int) = mkop(basis, op(basis), idx)

"""
    mkid(basis::Basis)

Create a multi-mode identity operator in `basis`.
"""
mkid(basis::Basis) = mkop(basis, id(basis), 1)

"""
    operators(basis::Basis{S,M}, sys::System{S,M})

Create `h0` and `V` operators in `basis` for the system described by `sys`.
"""
function operators(basis::Basis{S,M}, sys::System{S,M}) where {S,M}
    # It's simpler to populate the values into higher rank tensors.
    h0s = zeros(basis.dim1, basis.dim1, S, S)
    Vs = zeros(basis.dim1, basis.dim1, S, S)

    for s1 in 1:S
        for s2 in 1:S
            if s1 == s2
                h0s[:, :, s1, s1] .+= sys.energy[s1, s1] * mkid(basis)

                for m in 1:M
                    h0s[:, :, s1, s1] .+= sys.freq[m, s1] * (mkop(basis, n, m) + 0.5 * mkop(basis, id, m))
                end
            else
                Vs[:, :, s2, s1] .+= sys.energy[s2, s1] * mkid(basis)
            end

            for m in 1:M
                if s1 == s2
                    h0s[:, :, s1, s1] .+= sys.lin[m, s1, s1] * mkop(basis, q, m)
                else
                    Vs[:, :, s2, s1] .+= sys.lin[m, s2, s1] * mkop(basis, q, m)
                end
            end

            for m1 in 1:M
                for m2 in 1:M
                    Vs[:, :, s2, s1] .+= 0.5 * sys.quad[m2, m1, s2, s1] * mkop(basis, q, m2) * mkop(basis, q, m1)
                end
            end
        end
    end

    # Flatten into matrices.
    h0 = reshape(permutedims(h0s, [1, 3, 2, 4]), (basis.dim, basis.dim))
    maximum(abs.(h0' - h0)) < 1e-13 || @warn "Asymmetric h0: $(maximum(abs.(h0' - h0)))"
    V = reshape(permutedims(Vs, [1, 3, 2, 4]), (basis.dim, basis.dim))
    maximum(abs.(V' - V)) < 1e-13 || @warn "Asymmetric V: $(maximum(abs.(V' - V)))"

    h0, V
end


text:
"""

 █████╗  ██████╗██████╗ ██╗
██╔══██╗██╔════╝██╔══██╗██║
███████║██║     ██████╔╝██║
██╔══██║██║     ██╔══██╗██║
██║  ██║╚██████╗██║  ██║███████╗
╚═╝  ╚═╝ ╚═════╝╚═╝  ╚═╝╚══════╝

File:       PolyTrajGen.jl
Author:     Gabriel Barsi Haberfeld, 2020. gbh2@illinois.edu
Function:   This program implements minSnapTrajec_matlab by Clark-Youngdong-Son
            in Julia.

Instructions:   Run this file in juno with Julia 1.5.1 or later.
Requirements:   JuMP, COSMO, Plots, LinearAlgebra, Polynomials, ControlSystems,
                and BenchmarkTools

"""

using JuMP, COSMO
using Plots, LinearAlgebra
using Polynomials
using ControlSystems
using BenchmarkTools
using CSV

function computeTraj()
    m_q = 4                                #mass of a quadrotor
    I_q = diag([0.3 0.3 0.3])              #moment of inertia of a quadrotor
    g = 9.81                               #gravitational acceleration

    #Trajectory
    #keyframes, column = index, rows = xyz yaw
    keyframe = [
        0 7.1324 14.3516 7.9345
        0 8.6237 0.7840 -5.3136
        0 1.0192 1.3676 0.9495
        0 0 0 0
    ]
    m = size(keyframe, 2)
    n = 4                                  #number of flat outputs (x, y, z, psi)
    t_f = 15                               #final time of the trajectory

    order = 6                              #order of polynomial functions

    time_interval_selection_flag = true    #true : fixed time interval, false : optimal time interval
    if (time_interval_selection_flag)
        t = collect(range(0, stop = t_f, length = m + 1))
    end

    n_intermediate = 5
    corridor_width = 0.05
    corridor_position = [3 4]
    c = zeros(4 * (order + 1) * m)
    mu_r = 1
    mu_psi = 1
    k_r = 4
    k_psi = 2
    A = computeCostMat(order, m, mu_r, mu_psi, k_r, k_psi, t)
    C, b, Cin, bin = computeConstraint(
        order,
        m,
        3,
        2,
        t,
        keyframe,
        corridor_position,
        n_intermediate,
        corridor_width,
    )
    solution = optimizeTraj(A, C, b)
    PlotTraj(solution, m, t, keyframe, n)
end

mutable struct uavstate
    position
    angle
    speed
end

mutable struct waypoints
    position
    heading
    speed
    time
end

function a2bPoly(aState, bState, tfinal=10.0)
    waypoints1 = waypoints(aState.position, aState.angle[1], eul2rotmZYX(aState.angle)*aState.speed, 0.0)
    waypoints2 = waypoints(bState.position, bState.angle[1], eul2rotmZYX(bState.angle)*bState.speed, tfinal)
    r_xyz = 1; r_yaw = 1;
    H = zeros(18,18);    # 5 * 3 + 3 (4th order for xyz & 2nd order for yaw)
    H[1,1] = r_xyz;
    H[6,6] = r_xyz;
    H[11,11] = 10000*r_xyz;
    H[16,16] = r_yaw;
    # start & final constraints (equality)
    Aeq = [waypoints1.time^4 waypoints1.time^3 waypoints1.time^2 waypoints1.time 1 zeros(1,13)];            # x
    Aeq = [Aeq;[zeros(1,5) waypoints1.time^4 waypoints1.time^3 waypoints1.time^2 waypoints1.time 1 zeros(1,8)]];  # y
    Aeq = [Aeq;[zeros(1,10) waypoints1.time^4 waypoints1.time^3 waypoints1.time^2 waypoints1.time 1 zeros(1,3)]]; # z
    Aeq = [Aeq;[zeros(1,15) waypoints1.time^2 waypoints1.time 1]]; # yaw
    beq = [waypoints1.position[1];waypoints1.position[2];waypoints1.position[3];waypoints1.heading];

    Aeq = [Aeq;[waypoints2.time^4 waypoints2.time^3 waypoints2.time^2 waypoints2.time 1 zeros(1,13)]];            # x
    Aeq = [Aeq;[zeros(1,5) waypoints2.time^4 waypoints2.time^3 waypoints2.time^2 waypoints2.time 1 zeros(1,8)]];  # y
    Aeq = [Aeq;[zeros(1,10) waypoints2.time^4 waypoints2.time^3 waypoints2.time^2 waypoints2.time 1 zeros(1,3)]]; # z
    Aeq = [Aeq;[zeros(1,15) waypoints2.time^2 waypoints2.time 1]];                                                    # yaw
    beq = [beq;waypoints2.position[1];waypoints2.position[2];waypoints2.position[3];waypoints2.heading];

    # velocity of the start point
    Aeq = [Aeq;[4*waypoints1.time^3 3*waypoints1.time^2 2*waypoints1.time 1 0 zeros(1,13)]];                              # x'
    Aeq = [Aeq;[zeros(1,5) 4*waypoints1.time^3 3*waypoints1.time^2 2*waypoints1.time 1 0 zeros(1,8)]];                    # y'
    Aeq = [Aeq;[zeros(1,10) 4*waypoints1.time^3 3*waypoints1.time^2 2*waypoints1.time 1 0 zeros(1,3)]];                   # z'
    beq = [beq;waypoints1.speed[1];waypoints1.speed[2];waypoints1.speed[3]];
    # velocity of the final point
    Aeq = [Aeq;[cos(waypoints2.heading)*[4*waypoints2.time^3 3*waypoints2.time^2 2*waypoints2.time 1 0] sin(waypoints2.heading)*[4*waypoints2.time^3 3*waypoints2.time^2 2*waypoints2.time 1 0] zeros(1,8)]];
    Aeq = [Aeq;[sin(waypoints2.heading)*[4*waypoints2.time^3 3*waypoints2.time^2 2*waypoints2.time 1 0] -cos(waypoints2.heading)*[4*waypoints2.time^3 3*waypoints2.time^2 2*waypoints2.time 1 0] zeros(1,8)]];
    Aeq = [Aeq;[zeros(1,10) 4*waypoints2.time^3 3*waypoints2.time^2 2*waypoints2.time 1 0 zeros(1,3)]];                   # z'
    beq = [beq;-waypoints2.speed[1];waypoints2.speed[2];waypoints2.speed[3]];
    # solving quadratic problem
   sol = optimizeTraj(H,Aeq,beq)
   plota2bPoly(sol, tfinal, aState, bState)
   return sol
end

function computeCostMat(order, m, mu_r, mu_psi, k_r, k_psi, t)
    polynomial_r = Polynomial(ones(order + 1))
    for i = 1:k_r
        polynomial_r = derivative(polynomial_r)       #Differentiation up to k
    end
    polynomial_r = reverse(polynomial_r.coeffs)
    polynomial_psi = Polynomial(ones(order + 1))
    for i = 1:k_psi
        polynomial_psi = derivative(polynomial_psi)   #Differentiation up to k
    end
    polynomial_psi = reverse(polynomial_psi.coeffs)
    A = []
    for i = 1:m
        A_x = zeros(order + 1, order + 1)
        A_y = zeros(order + 1, order + 1)
        A_z = zeros(order + 1, order + 1)
        A_psi = zeros(order + 1, order + 1)
        for j = 1:order+1
            for k = j:order+1

                #Position
                if (j <= length(polynomial_r) && (k <= length(polynomial_r)))
                    order_t_r = ((order - k_r - j + 1) + (order - k_r - k + 1))
                    if (j == k)
                        A_x[j, k] =
                            polynomial_r[j]^2 / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                        A_y[j, k] =
                            polynomial_r[j]^2 / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                        A_z[j, k] =
                            polynomial_r[j]^2 / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                    else
                        A_x[j, k] =
                            2 * polynomial_r[j] * polynomial_r[k] / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                        A_y[j, k] =
                            2 * polynomial_r[j] * polynomial_r[k] / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                        A_z[j, k] =
                            2 * polynomial_r[j] * polynomial_r[k] / (order_t_r + 1) *
                            (t[i+1]^(order_t_r + 1) - t[i]^(order_t_r + 1))
                    end
                end

                #Yaw
                if (j <= length(polynomial_psi) && (k <= length(polynomial_psi)))
                    order_t_psi = ((order - k_psi - j + 1) + (order - k_psi - k + 1))
                    if (j == k)
                        A_psi[j, k] =
                            polynomial_psi[j]^2 / (order_t_psi + 1) *
                            (t[i+1]^(order_t_psi + 1) - t[i]^(order_t_psi + 1))
                    else
                        A_psi[j, k] =
                            2 * polynomial_psi[j] * polynomial_psi[k] / (order_t_psi + 1) *
                            (t[i+1]^(order_t_psi + 1) - t[i]^(order_t_psi + 1))
                    end
                end

            end
        end
        if i == 1
            blocks = [mu_r * A_x, mu_r * A_y, mu_r * A_z, mu_psi * A_psi]
        else
            blocks = [A, mu_r * A_x, mu_r * A_y, mu_r * A_z, mu_psi * A_psi]
        end
        A = ControlSystems.blockdiag(blocks...)
    end
    A = 0.5 * (A + A') #Make it symmetric
end

function computeConstraint(
    order,
    m,
    k_r,
    k_psi,
    t,
    keyframe,
    corridor_position,
    n_intermediate,
    corridor_width,
)

    n = 4                              #State number

    #Waypoint constraints
    C1 = zeros(2 * m * n, n * (order + 1) * m)
    b1 = zeros(2 * m * n)
    C = []
    b = []
    computeMat = diagm(ones(order + 1))          #Required for computation of polynomials
    for i = 1:m
        waypoint = keyframe[:, i]       #Waypoint at t(i)

        if (i == 1)                        #Initial and Final Position
            #Initial
            values = zeros(1, order + 1)
            for j = 1:order+1
                poly = Polynomial(computeMat[j, :])
                values[j] = poly(t[i])
            end
            values = reverse(values, dims = 2)
            for k = 1:n
                c = zeros(1, n * (order + 1) * m)
                c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                    values
                C1[k, :] = c
            end
            b1[1:n] = waypoint

            #Final
            for j = 1:order+1
                poly = Polynomial(computeMat[j, :])
                values[j] = poly(t[m+1])
            end
            values = reverse(values, dims = 2)
            for k = 1:n
                c = zeros(1, n * (order + 1) * m)
                c[((m-1)*(order+1)*n+(k-1)*(order+1)+1):((m-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                    values
                C1[k+n, :] = c
            end
            b1[n+1:2*n] = waypoint

        else
            #Elsewhere
            values = zeros(1, order + 1)
            for j = 1:order+1
                poly = Polynomial(computeMat[j, :])
                values[j] = poly(t[i])
            end
            values = reverse(values, dims = 2)
            for k = 1:n
                c = zeros(1, n * (order + 1) * m)
                c[((i-2)*(order+1)*n+(k-1)*(order+1)+1):((i-2)*(order+1)*n+(k-1)*(order+1))+order+1] =
                    values
                C1[k+2*n*(i-1), :] = c
            end
            b1[2*n*(i-1)+1:2*n*(i-1)+n] = waypoint

            for k = 1:n
                c = zeros(1, n * (order + 1) * m)
                c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                    values
                C1[k+2*n*(i-1)+n, :] = c
            end
            b1[2*n*(i-1)+n+1:2*n*(i-1)+2*n] = waypoint

        end

    end

    # Derivative constraints

    # Position
    C2 = zeros(2 * m * (n - 1) * k_r, n * (order + 1) * m)                    #(n-1) : yaw excluded here
    b2 = ones(2 * m * (n - 1) * k_r, 1) * eps(1.0)

    #Only for the quadrotor system
    #Position constraints
    constraintData_r = zeros(m, k_r, 3)
    #velocity
    if (k_r >= 1)
        constraintData_r[1, 1, 1:3] .= 0          #At starting position
        constraintData_r[2:m, 1, 1:2] .= eps(1.0)      #x,y velocities
        constraintData_r[2:m, 1, 3] .= eps(1.0)        #z velocity
    end
    #acceleration
    if (k_r >= 2)
        constraintData_r[1, 2, 3] = 0            #At starting position
        constraintData_r[2:m, 2, 1:2] .= eps(1.0)      #x,y accelerations
        constraintData_r[2:m, 2, 3] .= eps(1.0)        #z acceleration
    end
    #jerk
    if (k_r >= 3)
        #all zeros
    end
    #snap
    if (k_r >= 4)
        #all zeros
    end
    #Yaw constraints
    constraintData_psi = zeros(m, k_psi, 1)
    #velocity
    if (k_psi >= 1)
        constraintData_psi[1, 1, 1] = 0          #At starting position
    end
    #acceleration
    if (k_psi >= 2)
        #all zeros
    end
    #jerk
    if (k_psi >= 3)
        #all zeros
    end
    #snap
    if (k_psi >= 4)
        #all zeros
    end

    #constraintData_r = zeros(m,k_r,3);
    for i = 1:m
        for h = 1:k_r
            if (i == 1)
                #Initial
                values = zeros(1, order + 1)
                for j = 1:order+1
                    tempCoeffs = computeMat[j, :]
                    for k = 1:h
                        tempCoeffs = derivative(Polynomial(tempCoeffs)).coeffs
                    end
                    tempPoly = Polynomial(tempCoeffs)
                    values[j] = tempPoly(t[i])
                end
                values = reverse(values, dims = 2)
                continuity = zeros(1, n - 1)
                for k = 1:n-1
                    if (constraintData_r[i, h, k] == eps(1.0))
                        #Continuity
                        continuity[k] = true
                    end

                    c = zeros(1, n * (order + 1) * m)
                    if (continuity[k] == true)
                        c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        c[((m-1)*(order+1)*n+(k-1)*(order+1)+1):((m-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            -values
                        C2[k+(h-1)*(n-1), :] = c
                        b2[k+(h-1)*(n-1)] = 0
                    else
                        c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        C2[k+(h-1)*(n-1), :] = c
                        b2[k+(h-1)*(n-1)] = constraintData_r[i, h, k]
                    end
                end

                #Final
                values = zeros(1, order + 1)
                for j = 1:order+1
                    tempCoeffs = computeMat[j, :]
                    for k = 1:h
                        tempCoeffs = derivative(Polynomial(tempCoeffs)).coeffs
                    end
                    tempPoly = Polynomial(tempCoeffs)
                    values[j] = tempPoly(t[i])
                end
                values = reverse(values, dims = 2)
                for k = 1:n-1
                    if (constraintData_r[i, h, k] == eps(1.0))
                        #Continuity
                    end
                    c = zeros(1, n * (order + 1) * m)
                    if (continuity[k] == 0)
                        c[((m-1)*(order+1)*n+(k-1)*(order+1)+1):((m-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        C2[k+(h-1)*(n-1)+(n-1)*k_r, :] = c
                        b2[k+(h-1)*(n-1)+(n-1)*k_r] = constraintData_r[i, h, k]
                    end
                end

            else

                #Elsewhere
                values = zeros(1, order + 1)
                for j = 1:order+1
                    tempCoeffs = computeMat[j, :]
                    for k = 1:h
                        tempCoeffs = derivative(Polynomial(tempCoeffs)).coeffs
                    end
                    tempPoly = Polynomial(tempCoeffs)
                    values[j] = tempPoly(t[i])
                end
                values = reverse(values, dims = 2)
                continuity = zeros(1, n - 1)
                for k = 1:n-1
                    if (constraintData_r[i, h, k] == eps(1.0))
                        #Continuity
                        continuity[k] = true
                    end

                    c = zeros(1, n * (order + 1) * m)
                    if (continuity[k] == true)
                        c[((i-2)*(order+1)*n+(k-1)*(order+1)+1):((i-2)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            -values
                        C2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r, :] = c
                        b2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r] = 0
                    else
                        c[((i-2)*(order+1)*n+(k-1)*(order+1)+1):((i-2)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        C2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r, :] = c
                        b2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r] = constraintData_r[i, h, k]
                    end
                end

                continuity = zeros(1, n - 1)
                for k = 1:n-1
                    if (constraintData_r[i, h, k] == eps(1.0))
                        #Continuity
                        continuity[k] = true
                    end
                    c = zeros(1, n * (order + 1) * m)

                    if (continuity[k] == 0)
                        c[((i-1)*(order+1)*n+(k-1)*(order+1)+1):((i-1)*(order+1)*n+(k-1)*(order+1))+order+1] =
                            values
                        C2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r+(n-1)*k_r, :] = c
                        b2[k+(h-1)*(n-1)+2*(i-1)*(n-1)*k_r+(n-1)*k_r] =
                            constraintData_r[i, h, k]
                    end

                end

            end
        end
    end

    #Corridor constraints

    C3 = []

    b3 = []
    t_vector =
        (keyframe[1:3, corridor_position[2]] - keyframe[1:3, corridor_position[1]]) /
        norm(keyframe[1:3, corridor_position[2]] - keyframe[1:3, corridor_position[1]])
    #unit vector of direction of the corridor

    t_intermediate = collect(range(
        t[corridor_position[1]],
        stop = t[corridor_position[2]],
        length = n_intermediate + 2,
    ))
    t_intermediate = t_intermediate[2:end-1]
    #intermediate time stamps

    computeMat = diagm(ones(order + 1))#Required for computation of polynomials
    for i = 1:n_intermediate
        values = zeros(1, order + 1)
        for j = 1:order+1
            poly = Polynomial(computeMat[j, :])
            values[j] = poly(t_intermediate[i])
        end

        c = zeros(6, n * (order + 1) * m)       #Absolute value constraint : two inequality constraints
        b = zeros(6, 1)

        rix = keyframe[1, corridor_position[1]]
        riy = keyframe[2, corridor_position[1]]
        riz = keyframe[3, corridor_position[1]]
        #x
        c[
            1,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            [values zeros(1, 2 * (order + 1))] -
            t_vector[1] * [t_vector[1] * values t_vector[2] * values t_vector[3] * values]
        b[1] =
            corridor_width +
            rix +
            t_vector[1] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])
        c[
            2,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            -c[
                1,
                (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
            ]
        b[2] =
            corridor_width - rix -
            t_vector[1] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])
        #y
        c[
            3,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            [zeros(1, order + 1) values zeros(1, order + 1)] -
            t_vector[2] * [t_vector[1] * values t_vector[2] * values t_vector[3] * values]
        b[3] =
            corridor_width +
            riy +
            t_vector[2] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])
        c[
            4,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            -c[
                3,
                (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
            ]
        b[4] =
            corridor_width - riy -
            t_vector[2] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])
        #z
        c[
            5,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            [zeros(1, 2 * (order + 1)) values] -
            t_vector[3] * [t_vector[1] * values t_vector[2] * values t_vector[3] * values]
        b[5] =
            corridor_width +
            riz +
            t_vector[3] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])
        c[
            6,
            (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
        ] =
            -c[
                5,
                (corridor_position[1]-1)*n*(order+1)+0*(order+1)+1:(corridor_position[1]-1)*n*(order+1)+3*(order+1),
            ]
        b[6] =
            corridor_width - riz -
            t_vector[3] * (-rix * t_vector[1] - riy * t_vector[2] - riz * t_vector[3])

        if (i == 1)
            C3 = c
            b3 = b
        else
            C3 = [C3; c]
            b3 = [b3; b]
        end

        C = [C1; C2]
        b = [b1; b2]
    end
    return C, b, C3, b3
end

function optimizeTraj(A, C, b)
    A = 2 .* A
    m = JuMP.Model(with_optimizer(COSMO.Optimizer, max_iter = 5000, verbose = false))
    @variable(m, x[1:size(A, 1)])
    @objective(m, Min, 1 / 2 * x' * A * x)
    @constraint(m, C * x .== b)
    JuMP.optimize!(m)
    return value.(x)
end

function PlotTraj(solution, m, t, keyframe, n)

    dt = 0.01
    l = 0
    lm = length(t[i]:dt:t[i+1])
    l = m * lm
    x_trajec = zeros(l)
    y_trajec = zeros(l)
    z_trajec = zeros(l)
    psi_trajec = zeros(l)
    tvec = zeros(l)

    for i = 1:m
        idx1 = (i - 1) * lm + 1
        idx2 = i * lm
        x_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+0*(order+1):(i-1)*n*(order+1)+(order+1)+0*(order+1)],
            t[i]:dt:t[i+1],
        )
        y_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+1*(order+1):(i-1)*n*(order+1)+(order+1)+1*(order+1)],
            t[i]:dt:t[i+1],
        )
        z_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+2*(order+1):(i-1)*n*(order+1)+(order+1)+2*(order+1)],
            t[i]:dt:t[i+1],
        )
        psi_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+3*(order+1):(i-1)*n*(order+1)+(order+1)+3*(order+1)],
            t[i]:dt:t[i+1],
        )
        tvec[idx1:idx2] = Array(t[i]:dt:t[i+1])
    end

    l = @layout [a; b; c]
    default(dpi = 300)
    default(thickness_scaling = 2)
    default(size = [1200, 800])
    plot(tvec, x_trajec, lw = 3, ylabel = "x", label = nothing)
    p1 = plot!(t, keyframe[1, :], seriestype = :scatter, label = nothing)
    plot(tvec, y_trajec, lw = 3, ylabel = "y", label = nothing)
    p2 = plot!(t, keyframe[2, :], seriestype = :scatter, label = nothing)
    plot(tvec, z_trajec, lw = 3, xlabel = "Time [s]", ylabel = "z", label = nothing)
    p3 = plot!(t, keyframe[3, :], seriestype = :scatter, label = nothing)
    p = plot(p1, p2, p3, layout = l)
    display(p)
end

function writeCSV(solution, m, t, keyframe, n)
    dt = 0.01
    l = 0
    lm = length(t[i]:dt:t[i+1])
    l = m * lm
    x_trajec = zeros(l)
    y_trajec = zeros(l)
    z_trajec = zeros(l)
    psi_trajec = zeros(l)
    tvec = zeros(l)

    for i = 1:m

            solution[(i-1)*n*(order+1)+1+0*(order+1):(i-1)*n*(order+1)+(order+1)+0*(order+1)],
            t[i]:dt:t[i+1],
        )
        y_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+1*(order+1):(i-1)*n*(order+1)+(order+1)+1*(order+1)],
            t[i]:dt:t[i+1],
        )
        z_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+2*(order+1):(i-1)*n*(order+1)+(order+1)+2*(order+1)],
            t[i]:dt:t[i+1],
        )
        psi_trajec[idx1:idx2] = polyval(
            solution[(i-1)*n*(order+1)+1+3*(order+1):(i-1)*n*(order+1)+(order+1)+3*(order+1)],
            t[i]:dt:t[i+1],
        )
        tvec[idx1:idx2] = Array(t[i]:dt:t[i+1])
    end
end

function plota2bPoly(path_c, tfinal, aState, bState)
    tvec = 0:0.01:tfinal
    x_trajec =
        path_c[1] * tvec .^ 4 .+ path_c[2] * tvec .^ 3 .+ path_c[3] * tvec .^ 2 .+
        path_c[4] * tvec .+ path_c[5]
    y_trajec =
        path_c[6] * tvec .^ 4 .+ path_c[7] * tvec .^ 3 .+ path_c[8] * tvec .^ 2 .+
        path_c[9] * tvec .+ path_c[10]
    z_trajec =
        path_c[11] * tvec .^ 4 .+ path_c[12] * tvec .^ 3 .+ path_c[13] * tvec .^ 2 .+
        path_c[14] * tvec .+ path_c[15]
    yaw = path_c[16] * tvec .^ 2 .+ path_c[17] * tvec .+ path_c[18]
    default(dpi = 300)
    default(thickness_scaling = 2)
    default(size = [1200, 800])
    l = @layout [a; b; c]
    t = [0.0, tfinal]
    pos = [aState.position bState.position]
    p1 = plot(tvec, x_trajec, lw = 3, ylabel = "x", label = nothing)
    p1 = plot!(t, pos[1, :], seriestype = :scatter, label = nothing)
    p2 = plot(tvec, y_trajec, lw = 3, ylabel = "y", label = nothing)
    p2 = plot!(t, pos[2, :], seriestype = :scatter, label = nothing)
    p3 = plot(tvec, z_trajec, lw = 3, xlabel = "Time [s]", ylabel = "z", label = nothing)
    p3 = plot!(t, pos[3, :], seriestype = :scatter, label = nothing)
    p = plot(p1, p2, p3, layout = l)
    display(p)
    return nothing
end

function polyval(matlabPoly, tvec)
    #mimics matlabs polyval function
    poly = Polynomial(reverse(matlabPoly))
    poly.(tvec)
end

function eul2rotmZYX(eul)
    #mimics matlab
    ct = cos.(eul)
    st = sin.(eul)
    R = zeros(3, 3)
    R[1, 1] = ct[2] .* ct[1]
    R[1, 2] = st[3] .* st[2] .* ct[1] - ct[3] .* st[1]
    R[1, 3] = ct[3] .* st[2] .* ct[1] + st[3] .* st[1]
    R[2, 1] = ct[2] .* st[1]
    R[2, 2] = st[3] .* st[2] .* st[1] + ct[3] .* ct[1]
    R[2, 3] = ct[3] .* st[2] .* st[1] - st[3] .* ct[1]
    R[3, 1] = -st[2]
    R[3, 2] = st[3] .* ct[2]
    R[3, 3] = ct[3] .* ct[2]
    return R
end

max_stars_repo_name: gbarsih/planar-quad-mpc

max_issues_repo_name: gbarsih/planar-quad-mpc

max_forks_repo_name: gbarsih/planar-quad-mpc


text:
#
#
# General documentation of exp/log/... and its fallbacks in case of non-implemented tuples
#
#
import LinearAlgebra: norm
export addNoise, distance, dot, exp, getValue, log, manifoldDimension, norm
export manifoldDimension, parallelTransport, randomMPoint, randomTVector, tangentONB
export typeofMPoint, typeofTVector, typicalDistance, zeroTVector
export validateMPoint, validateTVector
@doc doc"""
    distance(M,x,y)
computes the gedoesic distance between two [`MPoint`](@ref)s `x` and `y` on
a [`Manifold`](@ref) `M`.
"""
function distance(M::mT, x::T, y::T) where {mT <: Manifold, T <: MPoint}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(y) )
  sig3 = string( typeof(M) )
  throw( DomainError("distance not defined/implemented for a $sig1 and a $sig2 on $sig3." ) )
end
@doc doc"""
    dot(M, x, ξ, ν)
Computes the inner product of two [`TVector`](@ref)s `ξ` and `ν` from the
tangent space at the [`MPoint`](@ref) `x` on the [`Manifold`](@ref) `M`.
"""
function dot(M::mT, x::P, ξ::T, ν::S) where {mT <: Manifold, P <: MPoint, T <: TVector, S <: TVector}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(ξ) )
  sig3 = string( typeof(ν) )
  sig4 = string( typeof(M) )
  throw( DomainError("dot not defined/implemented for a $sig2 and $sig3 in the tangent space of a $sig1 on $sig4." ) )
end
# fallback for forgetting base
function dot(M::mT, ξ::T, ν::S) where {mT <: Manifold, P <: MPoint, T <: TVector, S <: TVector}
  sig2 = string( typeof(ξ) )
  sig3 = string( typeof(ν) )
  sig4 = string( typeof(M) )
  throw( ErrorException("dot requires a base point, but the function call only contains a manifold ($sig4) and two tangent vectors ($sig2) and ($sig3)." ) )
end
"""
    exp(M,x,ξ,[t=1.0])
computes the exponential map at an [`MPoint`](@ref) `x` for the
[`TVector`](@ref) `ξ` on the [`Manifold`](@ref) `M`. The optional parameter `t` can be
used to shorten `ξ` to `tξ`.
"""
function exp(M::mT, x::P, ξ::T,t::Float64=1.0) where {mT<:Manifold, P<:MPoint, T<:TVector, N<:Number}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(ξ) )
  sig3 = string( typeof(M) )
  throw( DomainError("exp not defined/implemented for a $sig1 and a $sig2 on $sig3." ) )
end
@doc doc"""
    log(M,x,y)
computes the [`TVector`](@ref) in the tangent space $T_x\mathcal M$ at the
[`MPoint`](@ref) `x` such that the corresponding geodesic reaches the
[`MPoint`](@ref) `y` after time 1 on the [`Manifold`](@ref) `M`.
"""
function log(M::mT,x::P,y::Q)::TVector where {mT<:Manifold, P<:MPoint, Q<:MPoint}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(y) )
  sig3 = string( typeof(M) )
  throw( DomainError("log – not defined/implemented for a $sig1 and a $sig2 on $sig3.") )
end

@doc doc"""
    parallelTransport(M,x,y,ξ)
Parallel transport of a vector `ξ` given at the tangent space $T_x\mathcal M$
of `x` to the tangent space $T_y\mathcal M$ at `y` along the geodesic form `x` to `y`.
If the geodesic is not unique, this function takes the same choice as `geodesic`.
"""
function parallelTransport(M::mT, x::P, y::Q, ξ::T) where {mT<:Manifold, P<:MPoint, Q<:MPoint, T<:TVector}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(y) )
  sig3 = string( typeof(ξ) )
  sig4 = string( typeof(M) )
  throw( DomainError("parallelTransport not defined/implemented for a $sig1, a $sig2, and a $sig3 on $sig4." ) )
end

@doc doc"""
    (Ξ,κ) = tangentONB(M,x,ξ)

compute an ONB within the tangent space $T_x\mathcal M$ such that $\xi$ is the
first vector and compute the eigenvalues of the curvature tensor
$R(\Xi,\dot g)\dot g$, where $g=g_{x,\xi}$ is the geodesic with $g(0)=x$,
$\dot g(0) = \xi$, i.e. $\kappa_1$ corresponding to $\Xi_1=\xi$ is zero.

# See also
 [`jacobiField`](@ref), [`adjointJacobiField`](@ref).
"""
function tangentONB(M::mT, x::P, ξ::T) where {mT <: Manifold, P <: MPoint, T <: TVector}
    sig1 = string( typeof(x) )
    sig2 = string( typeof(ξ) )
    sig3 = string( typeof(M) )
    throw( DomainError("tangentONB not defined/implemented for a $sig1 and a $sig2 on $sig3." ) )
end

@doc doc"""
    (Ξ,κ) = tangentONB(M,x,y)

compute an ONB within the tangent space $T_x\mathcal M$ such that $\xi=\log_xy$ is the
first vector and compute the eigenvalues of the curvature tensor
$R(\Xi,\dot g)\dot g$, where $g=g_{x,\xi}$ is the geodesic with $g(0)=x$,
$\dot g(0) = \xi$, i.e. $\kappa_1$ corresponding to $\Xi_1=\xi$ is zero.

# See also
 [`jacobiField`](@ref), [`adjointJacobiField`](@ref).
"""
tangentONB(M::mT, x::P, y::Q) where {mT <: Manifold, P <: MPoint, Q <: MPoint} = tangentONB(M,x,log(M,x,y))
"""
    typeofMPoint(T)

return the [`MPoint`](@ref) belonging to the [`TVector`](@ref) type `T`.
"""
function typeofMPoint(ξT::Type{T}) where { T <: TVector }
    sig = string( ξT )
    throw( ErrorException("typeofMPoint not yet implemented for the tangent vector type $sig.") )
end
"""
    typeofTVector(P)

returns the [`TVector`](@ref) belonging to the [`MPoint`](@ref) type `P`.
"""
function typeofTVector(pP::Type{P}) where {P <: MPoint}
    sig = string( pP )
    throw( ErrorException("typeofTVector not yet implemented for the tangent vector type $sig.") )
end
"""
    typicalDistance(M)

returns the typical distance on the [`Manifold`](@ref) `M`, which is for example
the longest distance in a unit cell or injectivity radius.
"""
function typicalDistance(M::mT) where {mT <: Manifold}
  sig2 = string( typeof(M) )
  throw( ErrorException("typicalDistance(M) not implemented on $sig2." ) )
end
@doc doc"""
    validateMPoint(M,x)

check, whether the data in the [`MPoint`](@ref) `x` is a valid point on the
[`Manifold`](@ref) `M`. This is used to validate parameters and results during
computations using [`MPointE`](@ref)s.
Note that the default fallback is just a warning that no validation is available.

The function should throw an error if `x` is not point on the manifold `M`,
otherwise it should return `true`.
"""
function validateMPoint(M::mT, x::P) where {mT <: Manifold, P <: MPoint}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(M) )
  @warn """No valitadion for a $sig1 on $sig2 available. Continuing without
  validation. To turn this warning off, either deactivate the validate flag
  in (one of) your extended MPoints or implement a corresponding validation."""
  return true
end
@doc doc"""
    validateTVector(M,x,ξ)

check, whether the data in the [`TVector`](@ref) `ξ` is a valid tangent vector
#to the [`MPoint`](@ref) `x` on the [`Manifold`](@ref) `M`.
This is used to validate parameters and results during computations when using
[`MPointE`](@ref)s.
Note that the default fallback is just a warning that no validation is available.

Available validations should throw an error if `x` is not on `M` or `ξ` is not
in the tangent space of `x`. If `ξ` is valid, the function returns true.
"""
function validateTVector(M::mT, x::P, ξ::T) where {mT <: Manifold, P <: MPoint, T <: TVector}
  sig1 = string( typeof(x) )
  sig2 = string( typeof(ξ) )
  sig3 = string( typeof(M) )
  @warn """No valitadion for a $sig1 and a $sig2 on $sig3 available.
  Continuing without validation. To turn this warning off, either deactivate
  the validate flag in (one of) your extended TVectors or MPoints or
  implement a corresponding validation"""
  return true
end
@doc doc"""
    ξ = zeroTVector(M,x)

returns a zero vector in the tangent space $T_x\mathcal M$ of the
[`MPoint`](@ref) $x\in\mathcal M$ on the [`Manifold`](@ref) `M`.
"""
function zeroTVector(M::mT, x::P) where {mT <: Manifold, P <: MPoint}
    sig1 = string( typeof(x) )
    sig2 = string( typeof(M) )
    throw( DomainError("zeroTVector not defined/implemented for a $sig1 on $sig2." ) )
end


text:
using LinearAlgebra

struct Field{T,N,A}
    data::Array{T, N}
    grid::NTuple{N, A}
end

function Field(data, grid, units)
    Field(data*units, grid)
end

function Field(x::AbstractArray{T,N}, y::AbstractArray{T,N}, grid, units) where {T,N}
    data = Vec{2,T}.(x, y)
    Field(data*units, grid)
end

function Field(x::AbstractArray{T,N}, y::AbstractArray{T,N}, z::AbstractArray{T,N}, grid, units) where {T,N}
    data = Vec{3,T}.(x, y, z)
    Field(data*units, grid)
end

function Field(fr::PyObject, key::Symbol)
    py_obj = getproperty(fr, key)
    data = convert_it(py_obj.data)
    data_size = size(data)
    units = get_units(py_obj.units)
    grid = convert_it.(py_obj.grid.data)
    if data_size == length.(grid)
        Field(data, grid, units)
    else
        grid_mid = convert_it.(py_obj.grid_mid.data)
        Field(data, grid_mid, units)
    end
end

for f in (:+, :-, :*, :/)
    @eval function (Base.$f)(f1::Field, f2::Field)
        Field(($f).(f1.data, f2.data), f1.grid)
    end
end

function LinearAlgebra.cross(f1::Field, f2::Field)
    Field(cross.(f1.data, f2.data), f1.grid)
end

LinearAlgebra.norm(field::Field) = Field(norm.(field.data), field.grid)


text:
using JuliaFEM
using JuliaFEM.Preprocess
using JuliaFEM.Postprocess
using JuliaFEM.Abaqus: create_surface_elements

Logging.configure(level=Logging.DEBUG)

function create_body(mesh, name, E, nu, rho)
    body = Problem(mesh, Elasticity, name, 3)
    update!(body, "youngs modulus", E)
    update!(body, "poissons ratio", nu)
    update!(body, "density", rho)
    return body
end

""" Create boundary condition from surface set. """
function create_bc_from_surface_set(mesh, name, u1, u2, u3)
    bc = Problem(Dirichlet, string(name), 3, "displacement")
    bc.elements = create_surface_elements(mesh, name)
    update!(bc, "displacement 1", u1)
    update!(bc, "displacement 2", u2)
    update!(bc, "displacement 3", u3)
    return bc
end

""" Create boundary condition from node set. """
function create_bc_from_node_set(mesh, name, u1, u2, u3)
    bc = Problem(Dirichlet, string(name), 3, "displacement")
    bc.elements = [Element(Poi1, [nid]) for nid in mesh.node_sets[name]]
    update!(bc, "geometry", mesh.nodes)
    update!(bc, "displacement 1", u1)
    update!(bc, "displacement 2", u2)
    update!(bc, "displacement 3", u3)
    return bc
end

function create_bc(mesh, name, u1=0.0, u2=0.0, u3=0.0)
    name = Symbol(name)
    if haskey(mesh.surface_sets, name)
        return create_bc_from_surface_set(mesh, name, u1, u2, u3)
    elseif haskey(mesh.node_sets, name)
        return create_bc_from_node_set(mesh, name, u1, u2, u3)
    else
        error("Mesh does not contain node or surface set $name")
    end
end

function create_interface(mesh, slave_surface::String, master_surface::String)
    interface = Problem(Mortar, "interface between $slave_surface and $master_surface", 3, "displacement")
    interface.properties.dual_basis = true
    slave_elements = create_surface_elements(mesh, Symbol(slave_surface))
    master_elements = create_surface_elements(mesh, Symbol(master_surface))
    nslaves = length(slave_elements)
    nmasters = length(master_elements)
    info("$nslaves slaves, $nmasters masters")
    update!(slave_elements, "master elements", master_elements)
    interface.elements = [slave_elements; master_elements]
    return interface
end

function create_interface(mesh, slave::Problem, master::Problem)
    slave_surface = slave.name * "_TO_" * master.name
    master_surface = master.name * "_TO_" * slave.name
    return create_interface(mesh, slave_surface, master_surface)
end

""" Convert Mesh object from quadratic to linear. """
function to_linear!(mesh)
    mapping = Dict(:Tet10 => :Tet4, :Tri6 => :Tri3)
    nnodes = Dict(:Tet4 => 4, :Tri3 => 3)
    for elid in keys(mesh.elements)
        eltype = mesh.element_types[elid]
        if haskey(mapping, eltype)
            mesh.element_types[elid] = mapping[eltype]
            nnodes_new = nnodes[mapping[eltype]]
            mesh.elements[elid] = mesh.elements[elid][1:nnodes_new]
        end
    end
end

# start of simulation
#mesh = abaqus_read_mesh("beam_3d_1st_order_tetra_30mm.inp")
mesh = abaqus_read_mesh("beam_3d_2nd_order_tetra_30mm.inp")
#to_linear!(mesh)
info("element sets = ", collect(keys(mesh.element_sets)))
info("surface sets = ", collect(keys(mesh.surface_sets)))

# parts
beam = create_body(mesh, "beam", 210.0e3, 0.3, 7.85e-9)

# boundary conditions
bc1 = create_bc(mesh, "fixed")

# load
load = Problem(Elasticity, "pressure load", 3)
load.elements = create_surface_elements(mesh, :load)
nload = length(load.elements)
info("$nload elements in load surface")
area = 0.0
time = 0.0
for element in load.elements
    for ip in get_integration_points(element)
        detJ = element(ip, time, Val{:detJ})
        area += ip.weight*detJ
    end
end
info("load surface area: $area")

update!(load, "surface pressure", 20.0)

# solution
isfile("results.h5") && rm("results.h5")
isfile("results.xmf") && rm("results.xmf")
solver = Solver(Linear, beam, bc1, load)
solver.xdmf = Xdmf("results")
solver()

# calculate stresses in integration points and use least-squares fitting to
# extrapolate results to nodes

""" Return stress tensor. """
function get_stress_tensor(element, ip, time)
    haskey(element, "displacement") || return nothing
    gradu = element("displacement", ip, time, Val{:Grad})
    eps = 0.5*(gradu' + gradu)
    E = element("youngs modulus", ip, time)
    nu = element("poissons ratio", ip, time)
    mu = E/(2.0*(1.0+nu))
    la = E*nu/((1.0+nu)*(1.0-2.0*nu))
    S = la*trace(eps)*I + 2.0*mu*eps
    return S
end

""" Return stress vector in "ABAQUS" order 11, 22, 33, 12, 23, 13. """
function get_stress(element, ip, time)
    S = get_stress_tensor(element, ip, time)
    return [S[1,1], S[2,2], S[3,3], S[1,2], S[2,3], S[1,3]]
end

""" Return principal stresses. """
function get_stress_principal(element, ip, time)
    S = get_stress_tensor(element, ip, time)
    return sort(eigvals(S))
end

""" Make least squares fit for some field to nodes. """
function lsq_fit(elements, field)
    A = SparseMatrixCOO()
    b = SparseMatrixCOO()
    volume = 0.0
    for element in elements
        gdofs = get_connectivity(element)
        # increase integration order by 1 from default
        for ip in get_integration_points(element, 1)
	        detJ = element(ip, time, Val{:detJ})
	        w = ip.weight*detJ
	        N = element(ip, time)
	        f = field(element, ip, time)
	        add!(A, gdofs, gdofs, w*kron(N', N))
	        for i=1:length(f)
                add!(b, gdofs, w*f[i]*N, i)
            end
            volume += w
	    end
    end
    info("Mass matrix for least-squares fit is assembled. Total volume to fit: $volume")
    A = sparse(A)
    b = sparse(b)
    A = 1/2*(A + A')

    SparseArrays.droptol!(A, 1.0e-6)
    SparseArrays.dropzeros!(A)
    nz = get_nonzero_rows(A)
    F = ldltfact(A[nz,nz])

    x = zeros(size(b)...)
    x[nz, :] = F \ b[nz, :]

    nodal_values = Dict(i => vec(x[i,:]) for i in nz)
    return nodal_values
end

xdmf = get(solver.xdmf)
S = lsq_fit(beam.elements, get_stress)
#Sp = lsq_fit(beam.elements, get_stress_principal)

# Calculate principal stresses in nodes
Sp = Dict()
Sa = Dict()
Sm = Dict()
for (nid, s) in S
    # order is: 11, 22, 33, 12, 23, 13
    stress_tensor = [
        s[1] s[4] s[6]
        s[4] s[2] s[5]
        s[6] s[5] s[3]]
    principle = sort(eigvals(stress_tensor))
    Sp[nid] = principle
    Sa[nid] = (maximum(principle) - minimum(principle)) / 2
    Sm[nid] = (maximum(principle) + minimum(principle)) / 2
end

using JuliaFEM: new_dataitem, new_child, set_attribute, add_child, save!

""" Store values to xml/h5. """
function add_field_to_xdmf!(xdmf, field_name, path, data::Dict; field_type="Vector")
    node_ids = sort(collect(keys(data)))
    info("Storing field $field_name to path $path")
    println("$path in nodes:")
    for (i, nid) in enumerate(node_ids)
        println("$nid -> $(data[nid])")
        if i > 2
            println("...")
            break
        end
    end
    datavec = hcat([data[nid] for nid in node_ids]...)
    dataitem = new_dataitem(xdmf, path, datavec)
    frame = read(xdmf, "/Domain/Grid/Grid")
    attribute = new_child(frame, "Attribute")
    set_attribute(attribute, "Name", field_name)
    set_attribute(attribute, "Center", "Node")
    set_attribute(attribute, "AttributeType", field_type)
    add_child(attribute, dataitem)
end

add_field_to_xdmf!(xdmf, "Stress", "/Results/Time $(solver.time)/Nodal Fields/Stress", S; field_type="Tensor6")
add_field_to_xdmf!(xdmf, "Principal Stress", "/Results/Time $(solver.time)/Nodal Fields/Principal Stress", Sp; field_type="Vector")
add_field_to_xdmf!(xdmf, "Stress Amplitude", "/Results/Time $(solver.time)/Nodal Fields/Stress Amplitude", Sa; field_type="Scalar")
add_field_to_xdmf!(xdmf, "Mean Stress", "/Results/Time $(solver.time)/Nodal Fields/Mean Stress", Sm; field_type="Scalar")
save!(xdmf)

close(xdmf.hdf)


text:
module XtraMaps

export pmap!, map_with_indices, map_with_indices!, pmap_with_indices, pmap_with_indices!
export mmap_with_indices, mmap_with_indices!, pmmap_with_indices, pmmap_with_indices!
export szeros, sones, sfill

"""
Map over an array in place in parallel
"""
function pmap!(f::Function, lst::SharedArray)
  const np = nprocs();
  const n = length(lst);
  i = 1;
  nextidx() = (idx=i; i+=1; idx);
  @sync begin
    for p=1:np
      if p != myid() || np == 1
        @async begin
          while true
            idx = nextidx()
            if idx > n; break; end
            lst[idx] = remotecall_fetch(p, f, lst[idx]);
          end
        end
      end
    end
  end
end

"""
Map over an array using its index as the second argument
"""
function map_with_indices(f::Function, lst::Array)
  const n = length(lst);
  results = cell(n);
  for i=1:n
    results[i] = f(lst[i], i);
  end
  return results;
end

"""
Map over an array in place using its index as the second argument
"""
function map_with_indices!(f::Function, lst::Array)
  const n = length(lst);
  for i=1:n
    lst[i] = f(lst[i], i);
  end
end

"""
Map over an array in parallel with its index as the second argument
"""
function pmap_with_indices(f::Function, lst::Array)
  const np = nprocs();
  const n = length(lst);
  results = cell(n);
  i = 1;
  nextidx() = (idx=i; i+=1; idx);
  @sync begin
    for p=1:np
      if p != myid() || np == 1
        @async begin
          while true
            idx = nextidx()
            if idx > n; break; end
            results[idx] = remotecall_fetch(p, f, lst[idx], idx);
          end
        end
      end
    end
  end
  return results;
end

"""
Map over an array in place in parallel with its index as the second argument
"""
function pmap_with_indices!(f::Function, lst::SharedArray)
  const np = nprocs();
  const n = length(lst);
  i = 1;
  nextidx() = (idx=i; i+=1; idx);
  @sync begin
    for p=1:np
      if p != myid() || np == 1
        @async begin
          while true
            idx = nextidx()
            if idx > n; break; end
            lst[idx] = remotecall_fetch(p, f, lst[idx], idx);
          end
        end
      end
    end
  end
end

"""
Map over a 2D array with its indices as the second and third args
"""
function mmap_with_indices(f::Function, A::Array)
  const ni, nj = size(A);
  results = cell(ni, nj);

  for j=1:nj, i=1:ni
    results[i,j] = f(A[i,j], i, j);
  end
  return results;
end

"""
Map over a 2D array in place with its indices as the second and third args
"""
function mmap_with_indices!(f::Function, A::Array)
  const ni, nj = size(A);
  for j=1:nj, i=1:ni
    A[i,j] = f(A[i,j], i, j);
  end
end

"""
Map over a 2D array in parallel with its indices as the second and third args
"""
function pmmap_with_indices(f::Function, A::Array)
  const np = nprocs();
  const ni, nj = size(A);
  results = cell(ni, nj);
  i = 1; j = 1;

  nextidxs() = begin
    idx,jdx = i,j;
    i += 1
    if i > ni; i = 1; j += 1; end
    return idx, jdx;
  end

  @sync begin
    for p=1:np
      if p != myid() || np == 1
        @async begin
          while true
            idx,jdx = nextidxs();
            if jdx > nj; break; end
            results[idx,jdx] = remotecall_fetch(p, f, A[idx,jdx], idx, jdx);
          end
        end
      end
    end
  end
  return results;
end

"""
Map over a 2D array in place in parallel with its indices as 2nd and 3rd args
"""
function pmmap_with_indices!(f::Function, S::SharedArray)
  const np = nprocs();
  const ni, nj = size(S);
  i = 1; j = 1;

  nextidxs() = begin
    idx,jdx = i,j;
    i += 1
    if i > ni; i = 1; j += 1; end
    return idx, jdx;
  end

  @sync begin
    for p=1:np
      if p != myid() || np == 1
        @async begin
          while true
            idx,jdx = nextidxs();
            if jdx > nj; break; end
            S[idx,jdx] = remotecall_fetch(p, f, S[idx,jdx], idx, jdx);
          end
        end
      end
    end
  end
end

sones(dims) = SharedArray(Float64, dims, init = S -> S[localindexes(S)] = 1);
szeros(dims) = SharedArray(Float64, dims, init = S -> S[localindexes(S)] = 0);
sfill(fill, dims) = SharedArray(typeof(fill), dims, init = S -> S[localindexes(S)] = fill);

sones(t, dims) = SharedArray(t, dims, init = S -> S[localindexes(S)] = 1);
szeros(t, dims) = SharedArray(t, dims, init = S -> S[localindexes(S)] = 0);

end


text:
export ModulusRaised

"""
    ModulusRaised{P<:SHEShemeParams}

Modifies the underlying scheme to treat the last prime in the CRT basis as
a "special prime" to be used for modulus raising. This technique is used in
Microsoft SEAL (as of version 3.3) and described in https://eprint.iacr.org/2019/524.pdf.
Using the special prime reduces the noise introduced by keyswitching, without
requiring an (expensive) more fine grained relinerization radix.
"""
struct ModulusRaised{P<:SHEShemeParams} <: PassthroughParams{P}
    params::P
end

scheme_name(::Type{ModulusRaised{P}}) where P = string(scheme_name(P), " (with special prime)")

# The special prime is reserved for keys, so the ciphertext ring is the subring
# with the last crt component droppped.
ℛ_cipher(params::ModulusRaised) = drop_last(ℛ_cipher(parent_params(params)))
ℛ_plain(params::ModulusRaised{<:CKKSParams}) = drop_last(ℛ_plain(parent_params(params)))

function encrypt(rng::AbstractRNG, key::PubKey{P}, ::Zero) where {P<:ModulusRaised}
    c = encrypt(rng, PubKey(parent_params(key.params), key.key), Zero())
    CipherText{Zero}(key.params, map(modswitch_drop, c.cs))
end

function make_eval_key(rng::AbstractRNG, (old, new)::Pair{<:Any, P}) where {P<:PrivKey{<:ModulusRaised}}
    ps = modulus(moduli(eltype(eltype(new.secret))).parameters[end])
    parent_new = PrivKey(parent_params(new.params), new.secret)
    KeySwitchKey(new.params, make_eval_key(rng, ps*old=>parent_new).key)
end

function keyswitch_expand(ek::KeySwitchKey{<:ModulusRaised}, c)
    ℛkey = typeof(ek.key[1].mask)
    key_moduli = moduli(coefftype(ℛkey)).parameters
    𝔽ps = key_moduli[end]
    ℛexpanded = crtselect(typeof(ek.key[1].mask), [1:length(moduli(c).parameters); length(key_moduli)])
    ℛexpanded(c .* CRTExpand{𝔽ps}(), nothing)
end
keyswitch_contract(ek::KeySwitchKey{<:ModulusRaised}, c) = modswitch(c)
function downswitch_keyelement(params::ModulusRaised, key::KeyComponent, elt)
    which = [1:length(moduli(elt).parameters)-1; length(moduli(key.mask).parameters)]
    KeyComponent(
        crtselect(key.mask, which),
        crtselect(key.masked, which)
    )
end


text:
MOI.supports(::Optimizer, ::MOI.ObjectiveSense) = true

function MOIU.load(model::Optimizer, ::MOI.ObjectiveSense, sense::MOI.OptimizationSense)
    objective_info = model.inner.objective_info
    if sense == MOI.MIN_SENSE
        objective_info.sense = :Min
    elseif sense == MOI.MAX_SENSE
        objective_info.sense = :Max
    elseif sense == MOI.FEASIBILITY_SENSE
        objective_info.sense = :Feasibility
    else
        error("Unsupported objective sense: $sense")
    end
    return
end

MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{SAF}) = true
MOI.supports(::Optimizer, ::MOI.ObjectiveFunction{SQF}) = true

function MOIU.load(model::Optimizer, ::MOI.ObjectiveFunction{F}, obj::F) where {F<:Union{SAF, SQF}}
    model.inner.objective_info.expression = to_expr(obj)
    return
end


text:
# Automatically generated using Clang.jl wrap_c, version 0.0.0

export
    unix,
    linux,
    LIBAVDEVICE_VERSION_MAJOR,
    LIBAVDEVICE_VERSION_MINOR,
    LIBAVDEVICE_VERSION_MICRO,
    # LIBAVDEVICE_BUILD,
    AVDeviceRect,
    AVAppToDevMessageType,
    AV_APP_TO_DEV_NONE,
    AV_APP_TO_DEV_WINDOW_SIZE,
    AV_APP_TO_DEV_WINDOW_REPAINT,
    AV_APP_TO_DEV_PAUSE,
    AV_APP_TO_DEV_PLAY,
    AV_APP_TO_DEV_TOGGLE_PAUSE,
    AV_APP_TO_DEV_SET_VOLUME,
    AV_APP_TO_DEV_MUTE,
    AV_APP_TO_DEV_UNMUTE,
    AV_APP_TO_DEV_TOGGLE_MUTE,
    AV_APP_TO_DEV_GET_VOLUME,
    AV_APP_TO_DEV_GET_MUTE,
    AVDevToAppMessageType,
    AV_DEV_TO_APP_NONE,
    AV_DEV_TO_APP_CREATE_WINDOW_BUFFER,
    AV_DEV_TO_APP_PREPARE_WINDOW_BUFFER,
    AV_DEV_TO_APP_DISPLAY_WINDOW_BUFFER,
    AV_DEV_TO_APP_DESTROY_WINDOW_BUFFER,
    AV_DEV_TO_APP_BUFFER_OVERFLOW,
    AV_DEV_TO_APP_BUFFER_UNDERFLOW,
    AV_DEV_TO_APP_BUFFER_READABLE,
    AV_DEV_TO_APP_BUFFER_WRITABLE,
    AV_DEV_TO_APP_MUTE_STATE_CHANGED,
    AV_DEV_TO_APP_VOLUME_LEVEL_CHANGED,
    AVDeviceCapabilitiesQuery,
    AVDeviceInfo,
    AVDeviceInfoList

const unix = 1
const linux = 1
const LIBAVDEVICE_VERSION_MAJOR = 58
const LIBAVDEVICE_VERSION_MINOR = 5
const LIBAVDEVICE_VERSION_MICRO = 100

# Skipping MacroDefinition: LIBAVDEVICE_VERSION_INT AV_VERSION_INT ( LIBAVDEVICE_VERSION_MAJOR , LIBAVDEVICE_VERSION_MINOR , LIBAVDEVICE_VERSION_MICRO )
# Skipping MacroDefinition: LIBAVDEVICE_VERSION AV_VERSION ( LIBAVDEVICE_VERSION_MAJOR , LIBAVDEVICE_VERSION_MINOR , LIBAVDEVICE_VERSION_MICRO )

# const LIBAVDEVICE_BUILD = LIBAVDEVICE_VERSION_INT

# Skipping MacroDefinition: LIBAVDEVICE_IDENT "Lavd" AV_STRINGIFY ( LIBAVDEVICE_VERSION )

struct AVDeviceRect
    x::Cint
    y::Cint
    width::Cint
    height::Cint
end

# begin enum AVAppToDevMessageType
const AVAppToDevMessageType = UInt32
const AV_APP_TO_DEV_NONE = 1313820229 |> UInt32
const AV_APP_TO_DEV_WINDOW_SIZE = 1195724621 |> UInt32
const AV_APP_TO_DEV_WINDOW_REPAINT = 1380274241 |> UInt32
const AV_APP_TO_DEV_PAUSE = 1346458912 |> UInt32
const AV_APP_TO_DEV_PLAY = 1347174745 |> UInt32
const AV_APP_TO_DEV_TOGGLE_PAUSE = 1346458964 |> UInt32
const AV_APP_TO_DEV_SET_VOLUME = 1398165324 |> UInt32
const AV_APP_TO_DEV_MUTE = 541939028 |> UInt32
const AV_APP_TO_DEV_UNMUTE = 1431131476 |> UInt32
const AV_APP_TO_DEV_TOGGLE_MUTE = 1414354260 |> UInt32
const AV_APP_TO_DEV_GET_VOLUME = 1196838732 |> UInt32
const AV_APP_TO_DEV_GET_MUTE = 1196250452 |> UInt32
# end enum AVAppToDevMessageType

# begin enum AVDevToAppMessageType
const AVDevToAppMessageType = UInt32
const AV_DEV_TO_APP_NONE = 1313820229 |> UInt32
const AV_DEV_TO_APP_CREATE_WINDOW_BUFFER = 1111708229 |> UInt32
const AV_DEV_TO_APP_PREPARE_WINDOW_BUFFER = 1112560197 |> UInt32
const AV_DEV_TO_APP_DISPLAY_WINDOW_BUFFER = 1111771475 |> UInt32
const AV_DEV_TO_APP_DESTROY_WINDOW_BUFFER = 1111770451 |> UInt32
const AV_DEV_TO_APP_BUFFER_OVERFLOW = 1112491596 |> UInt32
const AV_DEV_TO_APP_BUFFER_UNDERFLOW = 1112884812 |> UInt32
const AV_DEV_TO_APP_BUFFER_READABLE = 1112687648 |> UInt32
const AV_DEV_TO_APP_BUFFER_WRITABLE = 1113018912 |> UInt32
const AV_DEV_TO_APP_MUTE_STATE_CHANGED = 1129141588 |> UInt32
const AV_DEV_TO_APP_VOLUME_LEVEL_CHANGED = 1129729868 |> UInt32
# end enum AVDevToAppMessageType

struct AVDeviceCapabilitiesQuery
    av_class::Ptr{AVClass}
    device_context::Ptr{AVFormatContext}
    codec::AVCodecID
    sample_format::AVSampleFormat
    pixel_format::AVPixelFormat
    sample_rate::Cint
    channels::Cint
    channel_layout::Int64
    window_width::Cint
    window_height::Cint
    frame_width::Cint
    frame_height::Cint
    fps::AVRational
end

struct AVDeviceInfo
    device_name::Cstring
    device_description::Cstring
end

struct AVDeviceInfoList
    devices::Ptr{Ptr{AVDeviceInfo}}
    nb_devices::Cint
    default_device::Cint
end


text:
@testset "Solve ODEs" begin
    "sample ode function"
    f_(u,p,t) = -1.01*u

    ExpDecay = DEsteady(func=f_, p=1.0, u0=3.0)

    # Ordinary solving
    prob = SteadyStateProblem(ExpDecay.func, ExpDecay.u0, ExpDecay.p)
    sol = solve(prob, ExpDecay.method)

    # With finc, u, p  wrapper
    sol_ = solveSS(ExpDecay.func, ExpDecay.u0, ExpDecay.p; method=ExpDecay.method )

    # With ode prameters
    sol_2 = solve(ExpDecay)

    # Solve Time-series
    de = ODEtime(func=f_, u0=3.0, p=1.0, tspan=(0.0,100.0), method= AutoTsit5(Rosenbrock23()))
    sol_time = solve(de)

    @test sol.u == sol_.u
    @test sol_2.u == sol.u
end

@testset "Threading Grid" begin
    f_(u,p,t) = -1.01*u
    us = ParameterGrid([(1.,3.,3)])

    de = DEsteady(func = f_, p=1.0, u0=us[1], method= Tsit5() )

    solve(de)

    @test length(solve(de, us)) == length(us) # mulit-threading mehod
end


text:
# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENSE in the project root.
# ------------------------------------------------------------------

"""
    EmpiricalVarioplane(data, var₁, var₂=var₁;
                        normal=spheredir(0,0),
                        nangs=50, ptol=0.5, dtol=0.5,
                        [parameters])

Given a `normal` direction, estimate the (cross-)variogram of variables
`var₁` and `var₂` along all directions in the corresponding plane of variation.

Optionally, specify the tolerance `ptol` for the plane partition, the tolerance
`dtol` for the direction partition, the number of angles `nangs` in the plane,
and forward the `parameters` to the underlying [`EmpiricalVariogram`](@ref).
"""
struct EmpiricalVarioplane{T,V}
  θs::Vector{T}
  γs::Vector{V}
end

function EmpiricalVarioplane(data, var₁::Symbol, var₂::Symbol=var₁;
                             normal=spheredir(0,0),
                             nangs=50, ptol=0.5, dtol=0.5,
                             kwargs...)
  # sanity checks
  @assert nangs > 1 "nangs must be greater than one"

  # deterministic results
  rng = MersenneTwister(123)

  # basis for variogram plane
  if embeddim(data) == 2
    planes = [data]
    u, v = Vec(1.,0.), Vec(0.,1.)
  elseif embeddim(data) == 3
    planes = partition(rng, data, PlanePartition(normal, tol=ptol))
    u, v = planebasis(normal)
  else
    @error "varioplane only supported in 2D or 3D"
  end

  # loop over half of the plane
  θs = range(0, stop=π, length=nangs)
  γs = map(θs) do θ
    dir = DirectionPartition(cos(θ)*u + sin(θ)*v, tol=dtol)

    γ(plane) = EmpiricalVariogram(partition(rng, plane, dir),
                                  var₁, var₂; kwargs...)
    foldxt(merge, Map(γ), collect(planes))
  end

  EmpiricalVarioplane(collect(θs), γs)
end

# ------------
# IO methods
# ------------
function Base.show(io::IO, γ::EmpiricalVarioplane)
  print(io, "EmpiricalVarioplane")
end

function Base.show(io::IO, ::MIME"text/plain", γ::EmpiricalVarioplane)
  θs = [@sprintf "%.2f" rad2deg(θ) for θ in γ.θs]
  ns = [sum(values(g)[3]) for g in γ.γs]
  lines = ["  └─$(θ)° → $n" for (θ, n) in zip(θs, ns)]
  lines = length(lines) > 11 ? vcat(lines[1:5],["  ⋮"],lines[end-4:end]) : lines
  println(io, γ)
  println(io, "  N° pairs")
  print(io, join(lines, "\n"))
end


text:
"""
    CategoricalLikelihood(l=BijectiveSimplexLink(softmax))

Categorical likelihood is to be used if we assume that the
uncertainty associated with the data follows a [Categorical distribution](https://en.wikipedia.org/wiki/Categorical_distribution).

Assuming a distribution with `n` categories:

## `n-1` inputs (bijective link)

One can work with a bijective transformation by wrapping a link (like `softmax`)
into a [`BijectiveSimplexLink`](@ref) and only needs `n-1` inputs:
```math
    p(y|f_1, f_2, \\dots, f_{n-1}) = \\operatorname{Categorical}(y | l(f_1, f_2, \\dots, f_{n-1}, 0))
```
The default constructor is a bijective link around `softmax`.

## `n` inputs (non-bijective link)

One can also pass directly the inputs without concatenating a `0`:
```math
    p(y|f_1, f_2, \\dots, f_n) = \\operatorname{Categorical}(y | l(f_1, f_2, \\dots, f_n))
```
This variant is over-parametrized, as there are `n-1` independent parameters
embedded in a `n` dimensional parameter space.
For more details, see the end of the section of this [Wikipedia link](https://en.wikipedia.org/wiki/Exponential_family#Table_of_distributions)
where it corresponds to Variant 1 and 2.
"""
struct CategoricalLikelihood{Tl<:AbstractLink} <: AbstractLikelihood
    invlink::Tl
end

CategoricalLikelihood(l=BijectiveSimplexLink(softmax)) = CategoricalLikelihood(link(l))

function (l::CategoricalLikelihood)(f::AbstractVector{<:Real})
    return Categorical(l.invlink(f))
end

(l::CategoricalLikelihood)(fs::AbstractVector) = Product(map(l, fs))


text:
@testset verbose = true "Unit Tests" begin

    @testset "Initialization" begin
        # Write your tests here.
        nenv = NatureEnv()
        @test nenv isa NatureEnv
        RLBase.reset!(nenv)
        @test length(nenv.players) == nenv.num_starting_players
        for p in nenv.players
            @test length(p.food_counts) == nenv.food_types
        end
        @test length(nenv.food_frames) == nenv.food_types
        for frame in nenv.food_frames
            @test size(frame) == nenv.world_size[1:2]
        end
    end

    @testset "Overrides" begin
        nenv = NatureEnv()
        RLBase.reset!(nenv)
        states = [Nature.state(nenv, i) for i in 1:length(nenv.players)]
        # Test states for all players are the same size
        @test size.(states) |> unique |> length == 1
        @test size(states[1])[1:2] ==  Tuple(2*nenv.window+1 for i in 1:2)
        @test sum(Nature.reward(nenv, p) for p in 1:length(nenv.players)) == 2f0
        @test !any(Nature.is_terminated(nenv, i) for i in 1:length(nenv.players))
        [Nature.action_space(nenv, i) for i in 1:length(nenv.players)]
    end

end


text:
using .DataValues

# DataValue overloads for query.jl definitions
nondatavaluetype(::Type{DataValue{T}}) where {T} = Union{T, Missing}
unwrap(x::DataValue) = isna(x) ? missing : DataValues.unsafe_get(x)
datavaluetype(::Type{T}) where {T <: DataValue} = T
datavaluetype(::Type{Union{T, Missing}}) where {T} = DataValue{T}
datavaluetype(::Type{Missing}) = DataValue{Union{}}
scalarconvert(T, x) = convert(T, x)
scalarconvert(::Type{T}, x::T) where {T} = x
scalarconvert(::Type{T}, ::Missing) where {T <: DataValue} = T()

@require CategoricalArrays="324d7699-5711-5eae-9e2f-1d82baa6b597" begin
    using .CategoricalArrays
    scalarconvert(::Type{DataValue{T}}, x::T) where {T <: CategoricalArrays.CatValue} = DataValue(x)
end

struct DataValueRowIterator{NT, S}
    x::S
end
DataValueRowIterator(::Type{NT}, x::S) where {NT <: NamedTuple, S} = DataValueRowIterator{NT, S}(x)

"Returns a DataValue-based NamedTuple-iterator"
DataValueRowIterator(::Type{Schema{names, types}}, x::S) where {names, types, S} = DataValueRowIterator{datavaluetype(NamedTuple{names, types}), S}(x)

function datavaluerows(x)
    r = Tables.rows(x)
    #TODO: add support for unknown schema
    return DataValueRowIterator(datavaluetype(Tables.schema(r)), r)
end

Base.eltype(rows::DataValueRowIterator{NT, S}) where {NT, S} = NT
Base.IteratorSize(::Type{DataValueRowIterator{NT, S}}) where {NT, S} = Base.IteratorSize(S)
Base.length(rows::DataValueRowIterator) = length(rows.x)

function Base.iterate(rows::DataValueRowIterator{NT, S}, st=()) where {NT <: NamedTuple{names}, S} where {names}
    if @generated
        vals = Tuple(:(scalarconvert($(fieldtype(NT, i)), getproperty(row, $(nondatavaluetype(fieldtype(NT, i))), $i, $(Meta.QuoteNode(names[i]))))) for i = 1:fieldcount(NT))
        q = quote
            x = iterate(rows.x, st...)
            x === nothing && return nothing
            row, st = x
            return $NT(($(vals...),)), (st,)
        end
        # @show q
        return q
    else
        x = iterate(rows.x, st...)
        x === nothing && return nothing
        row, st = x
        return NT(Tuple(scalarconvert(fieldtype(NT, i), getproperty(row, nondatavaluetype(fieldtype(NT, i)), i, names[i])) for i = 1:fieldcount(NT))), (st,)
    end
end

# Alternative lazy row implementation; currently though, Query.jl relies on
# being able to infer return types via all the type information of NamedTuples

# function Base.iterate(rows::DataValueRowIterator{NT}, st=()) where {NT}
#     state = iterate(rows.x, st...)
#     state === nothing && return nothing
#     row, st = state
#     return DataValueRow{NT, typeof(row)}(row), (st,)
# end

# struct DataValueRow{NT, T}
#     row::T
# end

# @inline Base.getproperty(dvr::DataValueRow{NamedTuple{names, types}}, nm::Symbol) where {names, types} = getproperty(dvr, Tables.columntype(names, types, nm), Tables.columnindex(names, nm), nm)
# @inline Base.getproperty(dvr::DataValueRow, ::Type{T}, col::Int, nm::Symbol) where {T} = T(getproperty(getfield(dvr, 1), nondatavaluetype(T), col, nm))