text:
struct SpriteElem{N,T}
    offset::Vec{2,T}
    uv_offset_width::Vec{4,T}
    position::Point{N, T}
    color::RGBA{Float32}
    scale::Vec{2,T}
end
mutable struct Text
    offsets
    uv_offset_width
    positions
    colors
    scales
    rotations
    atlas
end
Base.length(rt::Text) = length(rt.positions)
function Base.sizehint!(t::Text, size::Int)
    for elem in (:positions, :colors, :scales, :uv_offset_width, :offsets, :rotations)
        field = getfield(t, elem)
        if isa(field, AbstractArray) # all fields can be scalars
            sizehint!(field, size)
        end
    end
    nothing
end
function Base.resize!(t::Text, size::Int)
    for elem in (:positions, :colors, :scales, :uv_offset_width, :offsets, :rotations)
        field = getfield(t, elem)
        if isa(field, AbstractArray)
            resize!(field, size)
        end
    end
    nothing
end
function Base.setindex!(t::Text, value::NTuple{6}, i::Integer)
    char, offset, position, color, scale, rotation = value
    gs = GLVisualize.glyph_scale!(t.atlas, char, GLVisualize.defaultfont())
    guvw = GLVisualize.glyph_uv_width!(t.atlas, char, GLVisualize.defaultfont())
    t.offsets[i] = offset
    t.uv_offset_width[i] = guvw
    t.positions[i] = position
    t.colors[i] = color
    t.scales[i] = gs .* Vec2f0(scale)
    t.rotations[i] = rotation
    nothing
end

function Base.setindex!(t::Text, value::Char, i::Integer)
    gs = GLVisualize.glyph_scale!(rt.text.atlas, c, GLVisualize.defaultfont())
    guvw = GLVisualize.glyph_uv_width!(rt.text.atlas, c, GLVisualize.defaultfont())
    t.uv_offset_width[i] = guvw
    t.scales[i] = gs
end

function Base.setindex!(t::Text, value::Associative, i::Integer)
    for (k,v) in value
        getfield(t, k)[i] = v
    end
end
function Base.setindex!(t::Text, value::SpriteElem, i::Integer)
    for f=1:5
        getfield(t, f)[i] = getfield(value, f)
    end
end
function Text(robj::RenderObject)
    @materialize atlas, offset, uv_offset_width, position, color, scale, rotation = robj.uniforms
    text = Text(
        offset,
        uv_offset_width,
        position,
        color,
        scale,
        rotation,
        atlas
    )
end
mutable struct RichText
    text::Text
    inlined_objects::Vector
    links::Vector
    cursor
    defaults::Dict
end
function RichText(text::Text)
    style_default = Dict(
        :color => RGBA{Float32}(0,0,0,1),
        :scale => Vec2f0(1),
        :font => GLVisualize.defaultfont()
    )
    RichText(text, [], [], 0:1, style_default)
end

function Base.sizehint!(t::RichText, size::Int)
    sizehint!(t.text, size)
    nothing
end
function Base.resize!(t::RichText, size::Int)
    resize!(t.text, size)
    nothing
end
Base.length(rt::RichText) = length(rt.text)
make_iter(x) = repeated(x)
make_iter(x::AbstractArray) = x

function get_iter(defaultfunc, dictlike, key)
    make_iter(get(defaultfunc, dictlike, key))
end

function getposition(text, text2, fonts, scales, start_pos)
    GLVisualize.calc_position(text2, start_pos, scales, fonts, text.text.atlas)
end
function getoffsets(text, text2, fonts, scales)
    GLVisualize.calc_offset(text2, scales, fonts, text.text.atlas)
end
font_color(text::RichText) = text.defaults[:color]
font(text::RichText) = text.defaults[:font]
font_size(text::RichText) = text.defaults[:scale]
"""
Inserts text at [`position`] with `style`
"""
function Base.insert!(text::RichText, text2::AbstractString, position=text.cursor, style=text.defaults)
    fonts     = get_iter(()->font(text), style, :font)
    scales    = get_iter(()->font_size(text), style, :scale)
    start_pos = get(style, :start_position, text.text.positions[max(last(text.cursor), 1)])
    positions = get_iter(()->getposition(text, text2, fonts, scales, start_pos), style, :position)
    offsets   = get_iter(()->getoffsets(text, text2, fonts, scales), style, :offset)
    colors    = get_iter(()->font_color(text), style, :color)
    rotation  = get_iter(()->Vec3f0(0,0,1), style, :rotation)

    pos = max(last(text.cursor), 1)
    if pos + length(text2) > length(text)
        resize!(text, pos + length(text2))
    end
    for elem in zip(text2, offsets, positions, colors, scales, rotation)
        text.text[pos] = elem
        pos += 1
    end
    nothing
end

iter_or_array(x) = repeated(x)
iter_or_array(x::Repeated) = x
iter_or_array(x::Array) = x
iter_or_array(x::Vector{Ptr{FreeType.FT_FaceRec}}) = repeated(x)

function calc_position(
        last_pos, start_pos,
        atlas, glyph, font,
        scale, lineheight = 1.5
    )
    advance_x, advance_y = glyph_advance!(atlas, glyph, font, scale)
    if isnewline(glyph)
        return Point2f0(start_pos[1], last_pos[2] - advance_y * lineheight) #reset to startx
    else
        return last_pos + Point2f0(advance_x, 0)
    end
end
function calc_position(glyphs, start_pos, scales, fonts, atlas)
    positions = zeros(Point2f0, length(glyphs))
    last_pos  = Point2f0(start_pos)
    s, f = iter_or_array(scales), iter_or_array(fonts)
    c1 = first(glyphs)
    for (i, (c2, scale, font)) in enumerate(zip(glyphs, s, f))
        c2 == '\r' && continue # stupid windows!
        positions[i] = last_pos
        last_pos = calc_position(last_pos, start_pos, atlas, c2, font, scale)
        #k = FreeTypeAbstraction.kerning(c1, c2, font, scale)
        #last_pos += k # add kerning
    end
    positions
end

function calc_offset(glyphs, scales, fonts, atlas)
    offsets = fill(Point2f0(0.0), length(glyphs))
    s, f = iter_or_array(scales), iter_or_array(fonts)
    c1 = first(glyphs)
    for (i, (c2, scale, font)) in enumerate(zip(glyphs, s, f))
        c2 == '\r' && continue # stupid windows!
        offsets[i] = Point2f0(glyph_bearing!(atlas, c2, font, scale))
        c1 = c2
    end
    offsets # bearing is the glyph offset
end

isnewline(x) = x == '\n'
# i must be a valid character index
function next_newline(text, i::Integer)
    res = findnext(isnewline, text, i)
    res == 0 ? length(text) : res #jump to end of text if no newline found
end
previous_newline(text, i::Integer) = max(1, findprev(isnewline, text, i)) #jump to start

export previous_newline, next_newline

#=
textextetext\n
texttext<current pos>texttext\n
texttext<finds this pos>text\n
=#
function down_after_newline(text, current_position)
    i     = current_position
    pnl = previous_newline(text, i)
    nnl = next_newline(text, i)
    nl_distance = i-pnl # distance from previous newline
    min(length(text), nnl+nl_distance)
end
#=
textexte<finds this pos>text\n
texttext<current pos>texttext\n
texttexttext\n
=#
function up_before_newline(text, current_position)
    i      = current_position
    pnl  = previous_newline(text, i)
    ppnl = previous_newline(text, max(1, pnl-1))
    nl_distance = i-pnl # distance from previous newline
    min(length(text), ppnl+nl_distance)
end

function next_arrow_selection(dir, text, first_index, last_index)
    (dir == :up)    && return up_before_newline(text, first_index)
    (dir == :down)  && return down_after_newline(text, last_index)
    (dir == :left)  && return max(1, first_index-1)
    (dir == :right) && return min(length(text)+1, last_index+1)
    -1
end
function move_cursor(t0, dir, mouseselection, text_selection, is_movingselection)
    text, selection = text_selection.text, text_selection.selection
    mouseselection0, selection0 = t0
    selection0 = selection
    mouseselection0 != mouseselection && return (mouseselection, mouseselection) # if mouse selection has changed, return the new position
    if selection0 != 0:0
        first_i   = first(selection0) # first is always valid, if its not zero
        # last is not valid, if selection is in between characters
        last_i    = isempty(selection0) ? first(selection0) : last(selection0) # if only single char selected use first, otherwise last position of selection
        nas       = next_arrow_selection(dir, text, first_i, last_i) # -1 for no new arrow selection
        if nas != -1
            if is_movingselection
                return (mouseselection, nas:last_i)
            else
                return (mouseselection, nas:0)
            end
        end
    end
    (mouseselection0, selection0)
end
#
#
# immutable TextCursor
#     selections::Vector{UnitRange}
# end
# type Text{T <: Integer}
#     text::Vector{T}
# end
# function Base.setindex!(text, index::TextCursor, newtext)
#     for selection in index.selections
#
#     end
# end
deletetext(_, text_selection) = deletetext(text_selection)
function deletetext(text_selection)
    selection, text = text_selection.selection, text_selection.text
    offset = 0
    if first(selection) > 0 && last(selection) > 0
        if single_selection(selection)
            splice!(text, last(selection))
            offset = -1
        else
            splice!(text, selection)
        end
        text_selection.selection = max(1, first(selection)+offset):0 # when text gets removed, selection will turn into single selection
    end
    nothing
end
export inserttext

function inserttext(text_selection, to_insert)
    selection, text = text_selection.selection, text_selection.text
    if first(selection) > 0 && first(selection) <= length(text)
        splice!(text, selection, to_insert)
        chars_added = length(to_insert)
        text_selection.selection = (first(selection)+chars_added):0 # when text gets removed, selection will turn into single selection
    end
    nothing
end

function insert_text(text, cursor, new_text)
    text[cursor] = new_text
    push!(text, text)
    push!(cursor, text)
end

function selection_click(index)

    push!(cursor, )
end
function selection_drag(index)
    push!(cursor.selection, index)
    push!(cursor, )
end
function selection_add(cursor, selection)
    push!(cursor.selection, selection)
end
function selection_reset(cursor)
    push!(cursor)
end

function textedit_signals(inputs, background, text)
    @materialize unicodeinput, selection, buttonspressed, arrow_navigation, mousedragdiff_objectid = inputs
    # create object which can globally hold the text and selection
    text_raw    = TextWithSelection(text[:glyphs], 0:0)
    text_edit   = Signal(text_raw)
    shift       = const_lift(in, GLFW.KEY_LEFT_SHIFT, buttonspressed)
    selection   = preserve(const_lift(
        last,
        foldp(
            move_cursor,
            (selection.value, selection.value),
            arrow_navigation, selection,
            text_edit,
            shift
        )
    ))

    is_text(x) = x[2][1] == background.id || x[2][1] == text.id
    selection  = filterwhen(
        const_lift(is_text, mousedragdiff_objectid),
        0:0, selection
    )
    preserve(const_lift(s->(text_edit.value.selection=s), selection)) # is there really no other way?!

    strg_v          = const_lift(==, buttonspressed, Config.shortcuts["paste"])
    strg_c          = const_lift(==, buttonspressed, Config.shortcuts["copy"])
    strg_x          = const_lift(==, buttonspressed, Config.shortcuts["cut"])
    enter_key       = const_lift(==, buttonspressed, [GLFW.KEY_ENTER])
    del             = const_lift(==, buttonspressed, [GLFW.KEY_BACKSPACE])

    enter_insert    = const_lift(insert_enter,   filterwhen(enter_key, true, enter_key))
    clipboard_copy  = const_lift(copyclipboard,  filterwhen(strg_c,    true, strg_v), text_edit)

    delete_text     = const_lift(deletetext,     filterwhen(del,       true, del),    text_edit)
    cut_text        = const_lift(cutclipboard,   filterwhen(strg_x,    true, strg_x), text_edit)

    clipboard_paste = const_lift(clipboardpaste, filterwhen(strg_v,    true, strg_v))

    text_gate       = const_lift(isnotempty, unicodeinput)
    unicode_input   = filterwhen(text_gate, Char['0'], unicodeinput)
    text_to_insert  = merge(clipboard_paste, unicode_input, enter_insert)
    text_to_insert  = const_lift(process_for_gl, text_to_insert)

    text_inserted   = const_lift(inserttext, text_edit, text_to_insert)

    text_updates    = merge(
        const_lift(return_nothing, text_inserted),
        const_lift(return_nothing, clipboard_copy),
        const_lift(return_nothing, delete_text),
        const_lift(return_nothing, cut_text),
        const_lift(return_nothing, selection)
    )
    text_selection_signal = sampleon(text_updates, text_edit)

    selection   = const_lift(x->x.selection,  text_selection_signal)
    text_sig    = const_lift(x->x.text,       text_selection_signal)

    preserve(const_lift(update_positions, text_sig, Signal(text), Signal(background[:style_index])))
    preserve(foldp(visualize_selection, 0:0, selection,    Signal(background[:style_index])))
    const_lift(String, text_sig), selection
end

"""
Displayes an array of points with text labels. This should get moved into GLVisualize!
usage:
```
    points = Signal(Point2f0[(0, 0), (100, 100), (200, 200)]) # Doesn't need to be a signal
    labels = Signal(["label", "string", "α ∧ ¬β"]) # Doesn't need to be a signal
    annotated_text(points, labels)
```
"""
function annotated_text(
        points, labels_s;
        color = RGBA(0f0,0f0,0f0,1f0),
        scale = 5mm,
        base_offset = Point2f0(scale / 2)
    )
    # make this work for Signal(points) as well as constant points
    points_s = isa(points, Signal) ? points : Signal(points)
    font, atlas = GLVisualize.defaultfont(), GLVisualize.get_texture_atlas()
    # to display a text particle system, one needs
    # 1, glyph segmentation_centroids, offset to the bottom, position in the texture atlas
    # and the scale of the glyph. Then for bg labels, we also save a width
    v0 = (Point2f0[], Point2f0[], Vec4f0[], Vec2f0[], Vec2f0[])
    text = map(points_s) do points
        labels = value(labels_s)
        if length(labels) != length(points)
            error("""
                Colors, labels and points must have the same length! Found:
                Points: $(length(points)), labels: $(length(labels))""")
        end
        n = mapreduce(length, +, labels)
        rpoints, offsets, uv_widths, scales, widths = v0
        if length(labels) != length(widths)
            resize!(widths, length(labels))
        end
        if n != length(rpoints) # only resize if size changed
            resize!(rpoints, n); resize!(offsets, n)
            resize!(uv_widths, n); resize!(scales, n)
        end
        idx = 1
        # glyph height of a long glyph
        glyph_height = glyph_scale!(atlas, '|', font, scale)[2]
        for (i, (start_pos, label)) in enumerate(zip(points, labels))
            last_pos = start_pos .+ base_offset
            for c in label # calculate segmentation_centroids for each character/glyph
                rpoints[idx] = last_pos
                last_pos = calc_position(last_pos, start_pos, atlas, c, font, scale)
                offsets[idx] = glyph_bearing!(atlas, c, font, scale)
                uv_widths[idx] = glyph_uv_width!(atlas, c, font)
                scales[idx] = glyph_scale!(atlas, c, font, scale)
                idx += 1
            end
            # calculate label lengths + extra width
            widths[i] = Vec2f0(last_pos[1] - start_pos[1] + 1mm, glyph_height + 1mm)
        end
        rpoints, offsets, uv_widths, scales, widths
    end
    viz = visualize(
        (DISTANCEFIELD, map(x->x[1], text)), # render segmentation_centroids as a distance field particle type
        offset = map(x->x[2], text),
        uv_offset_width = map(x->x[3], text),
        scale = map(x->x[4], text),
        color = color,
        # drop down to low level opengl, to always render over image! (there should be a better API for this)
        prerender = () -> begin
            glDisable(GL_DEPTH_TEST)
            glDepthMask(GL_TRUE)
            glDisable(GL_CULL_FACE)
            enabletransparency()
        end,
        distancefield = atlas.images
    )
    viz, map(last, text) # return viz and widths
end

# # i must be a valid character index
# function next_newline(text, i::Integer)
#     res = findnext(isnewline, text, i)
#     res == 0 ? length(text) : res
# end
# previous_newline(text, i::Integer) = max(1, findprev(isnewline, text, i))
#
# export previous_newline
# export next_newline
#
#
#
# #=
# textextetext\n
# texttext<current pos>texttext\n
# texttext<finds this pos>text\n
# =#
# function down_after_newline(text, current_position)
#     i     = current_position
#     pnl = previous_newline(text, i)
#     nnl = next_newline(text, i)
#     nl_distance = i-pnl # distance from previous newline
#     min(length(text), nnl+nl_distance)
# end
# #=
# textexte<finds this pos>text\n
# texttext<current pos>texttext\n
# texttexttext\n
# =#
# function up_before_newline(text, current_position)
#     i      = current_position
#     pnl  = previous_newline(text, i)
#     ppnl = previous_newline(text, max(1, pnl-1))
#     nl_distance = i-pnl # distance from previous newline
#     min(length(text), ppnl+nl_distance)
# end
#
# function next_arrow_selection(dir, text, first_index, last_index)
#     (dir == :up)    && return up_before_newline(text, first_index)
#     (dir == :down)  && return down_after_newline(text, last_index)
#     (dir == :left)  && return max(1, first_index-1)
#     (dir == :right) && return min(length(text)+1, last_index+1)
#     -1
# end
#
# function move_cursor(t0, dir, mouseselection, text_selection, is_movingselection)
#     text, selection = text_selection.text, text_selection.selection
#     mouseselection0, selection0 = t0
#     selection0 = selection
#     mouseselection0 != mouseselection && return (mouseselection, mouseselection) # if mouse selection has changed, return the new position
#     if selection0 != 0:0
#         first_i   = first(selection0) # first is always valid, if its not zero
#         # last is not valid, if selection is in between characters
#         last_i    = isempty(selection0) ? first(selection0) : last(selection0) # if only single char selected use first, otherwise last position of selection
#         nas       = next_arrow_selection(dir, text, first_i, last_i) # -1 for no new arrow selection
#         if nas != -1
#             if is_movingselection
#                 return (mouseselection, nas:last_i)
#             else
#                 return (mouseselection, nas:0)
#             end
#         end
#     end
#     (mouseselection0, selection0)
# end
# export move_cursor
#
#
# function visualize_selection(
#         last_selection::UnitRange{Int},
#         selection       ::UnitRange{Int},
#         style           ::GPUVector{GLSpriteStyle}
#     )
#     fl, ll =  first(last_selection), last(last_selection)
#     if !isempty(last_selection) && fl > 0 && ll > 0 && (fl <= length(style)) && (ll <= length(style))
#         style[last_selection] = fill(GLSpriteStyle(0,0), length(last_selection))
#     end
#     fs, ls =  first(selection), last(selection)
#     if !isempty(selection) && fs > 0 && ls > 0 && (fs <= length(style)) && (ls <= length(style))
#         style[selection] = fill(GLSpriteStyle(1,0), length(selection))
#     end
#     selection
# end
#
#
# AND(a,b)       = a&&b
# isnotempty(x) = !isempty(x)
# return_nothing(x...) = nothing
# export AND
# export isnotempty
# export return_nothing
#
# single_selection(selection::UnitRange)     = isempty(selection) && first(selection)!=0
# is_textinput_modifiers(buttons::Vector{Int}) = isempty(buttons) || buttons == [GLFW.KEY_LEFT_SHIFT]
#
# function clipboardpaste(_)
#     clipboard_data = ""
#     try
#         clipboard_data = clipboard()
#     catch e # clipboard throws error when there is no data (WTF)
#     end
#     return String(clipboard_data)
# end
#
# export clipboardpaste
# export copyclipboard
#
# function back2julia(x::GLSprite)
#     isnewline(x[1]) && return '\n'
#     ID_TO_CHAR[x[1]]
# end
# function String(v::GPUVector{GLSprite})
#     data = gpu_data(v)
#     String(join(map(back2julia, data)))
# end
# # const_lift will have a boolean value at first argument position
# copyclipboard(_, text_selection) = copyclipboard(text_selection)
# function copyclipboard(text_selection)
#     selection, text = text_selection.selection, text_selection.text
#     if first(selection) > 0
#         if single_selection(selection) # for single selection we do a sublime style line copy
#             #i     = chr2ind(text, first(selection))
#             i     = min(length(text), first(selection)) # can be on position behind last character
#             pnl = previous_newline(text, i)
#             nnl = next_newline(text, i)
#             tocopy = text[pnl:nnl]
#         else # must be range selection
#             tocopy = text[selection]
#         end
#         clipboard(join(map(x->ID_TO_CHAR[x[1]], tocopy)))
#     end
#     nothing
# end
# export cutclipboard
# cutclipboard(_, text_selection) = cutclipboard(text_selection)
# function cutclipboard(text_selection)
#     copyclipboard(text_selection)
#     deletetext(text_selection)
#     nothing
# end
# export deletetext
# deletetext(_, text_selection) = deletetext(text_selection)
# function deletetext(text_selection)
#     selection, text = text_selection.selection, text_selection.text
#     offset = 0
#     if first(selection) > 0 && last(selection) > 0
#         if single_selection(selection)
#             splice!(text, last(selection))
#             offset = -1
#         else
#             splice!(text, selection)
#         end
#         text_selection.selection = max(1, first(selection)+offset):0 # when text gets removed, selection will turn into single selection
#     end
#     nothing
# end
# export inserttext
# inserttext(_, text_selection) = inserttext(text_selection)
# function inserttext(text_selection, to_insert)
#     selection, text = text_selection.selection, text_selection.text
#     if first(selection) > 0 && first(selection) <= length(text)
#         splice!(text, selection, to_insert)
#         chars_added = length(to_insert)
#         text_selection.selection = (first(selection)+chars_added):0 # when text gets removed, selection will turn into single selection
#     end
#     nothing
# end
#
# type TextWithSelection{S } #<: AbstractString}
#     text::S
#     selection::UnitRange{Int}
# end
# export TextWithSelection
#
#
#
#
# export visualize_selection


text:
# FWI on the 2D Overthrust model using spectral projected gradient descent
# Author: mlouboutin3@gatech.edu
# Date: February 2021
#

using DrWatson
@quickactivate :TimeProbeSeismic
import TimeProbeSeismic: qr_data

# Load starting model
# Load starting model
~isfile(datadir("models", "overthrust_model.h5")) && run(`curl -L ftp://slim.gatech.edu/data/SoftwareRelease/WaveformInversion.jl/2DFWI/overthrust_model_2D.h5 --create-dirs -o $(datadir("models", "overthrust_model.h5"))`)
n, d, o, m0, m = h5read(datadir("models", "overthrust_model.h5"), "n", "d", "o", "m0", "m")

n = Tuple(n)
o = Tuple(o)
d = Tuple(d)
m0[:, 20:end] = imfilter(m0[:, 20:end] ,Kernel.gaussian(5))
dm = vec(m - m0)

# Setup info and model structure
nsrc = 2	# number of sources
model = Model(n, d, o, m)
model0 = Model(n, d, o, m0)

# Set up receiver geometry
nxrec = n[1]
xrec = range(0f0, stop=(n[1] - 1)*d[1], length=nxrec)
yrec = 0f0
zrec = range(12.5f0, stop=12.5f0, length=nxrec)

# receiver sampling and recording time
timeD = 3000f0   # receiver recording time [ms]
dtD = 4f0  # receiver sampling interval [ms]

# Set up receiver structure
recGeometry = Geometry(xrec, yrec, zrec; dt=dtD, t=timeD, nsrc=nsrc)

# Set up source geometry (cell array with source locations for each shot)
xsrc = convertToCell(range(0f0, (n[1] - 1)*d[1]/2, length=nsrc))
ysrc = convertToCell(range(0f0, 0f0, length=nsrc))
zsrc = convertToCell(range(12.5f0, 12.5f0, length=nsrc))

# Set up source structure
srcGeometry = Geometry(xsrc, ysrc, zsrc; dt=dtD, t=timeD)

# setup wavelet
f0 = 0.010f0     # kHz
wavelet = ricker_wavelet(timeD, dtD, f0)
q = judiVector(srcGeometry, wavelet)

# Set up info structure for linear operators
ntComp = get_computational_nt(srcGeometry, recGeometry, model)
info = Info(prod(n), nsrc, ntComp)

###################################################################################################
# Write shots as segy files to disk
opt = Options(space_order=16)

# Setup operators
F = judiModeling(info, model, srcGeometry, recGeometry; options=opt)
F0 = judiModeling(info, model0, srcGeometry, recGeometry; options=opt)
J = judiJacobian(F0, q)

# data
# Nonlinear modeling
dobs = F*q
d0 = F0*q
residual = d0 - dobs
δd = J*dm

# gradient
g = Array{Any}(undef, 2)
g[1] = J[1]'*residual[1]
g[2] = J[2]'*residual[2]

# Probe
Jp = judiJacobian(F0, q, 2^1, dobs)
ge = Array{Any}(undef, 8, 2)
for r=1:8
    set_r!(Jp, 2^r)
    ge[r, 1] = Jp[1]'*residual[1]
    ge[r, 2] = Jp[2]'*residual[2]
end

for i=1:2
    clip = maximum(g[i])/10
    figure(figsize=(12, 8))
    subplot(331)
    imshow(g[i]', cmap="seismic", vmin=-clip, vmax=clip, aspect="auto")
    title("Reference")
    for r=1:8
        subplot(3,3,r+1)
        imshow(ge[r, i]', cmap="seismic", vmin=-clip, vmax=clip, aspect="auto")
        title("r=$(2^r)")
    end
    tight_layout()
    wsave(plotsdir("overthrust_single", "Probed_grad$i.png"), gcf())

    figure(figsize=(12, 8))
    subplot(331)
    imshow(g[i]', cmap="seismic", vmin=-clip, vmax=clip, aspect="auto")
    title("Reference")
    for r=1:8
        subplot(3,3,r+1)
        imshow(g[i]' - ge[r,i]', cmap="seismic", vmin=-clip, vmax=clip, aspect="auto")
        title("Difference r=$(2^r)")
    end
    tight_layout()
    wsave(plotsdir("overthrust_single", "Probed_err$i.png"), gcf())
end

# Similarities
similar = [simil(g[1], ge[i, 1]) for i=1:8]
similar2 = [simil(g[2], ge[i, 2]) for i=1:8]
# Similarities with muted wated
mw(x::PhysicalParameter) = mw(x.data)
mw(x::Array) = x[:, 19:end]
similar1 = [simil(mw(g[1]), mw(ge[i, 1])) for i=1:8]
similar12 = [simil(mw(g[2]), mw(ge[i, 2])) for i=1:8]

figure()
semilogx([2^i for i=1:8], similar, "-or", label="Full gradient turning", basex=2)
semilogx([2^i for i=1:8], similar2, "-ob", label="Full gradient reflections", basex=2)
semilogx([2^i for i=1:8], similar1, "-ob", label="Muted water layer turning", basex=2)
semilogx([2^i for i=1:8], similar12, "-ob", label="Muted water layer reflections", basex=2)
title(L"Similarity $\frac{<g, g_e>}{||g|| ||g_e||}$")
xlabel("Number of probing vectors")
legend()
wsave(plotsdir("overthrust_single", "simil.png"), gcf())

# Adjoint test
at = Array{Any}(undef, 8, 2)
J0 = dot(δd, residual)
for i=1:2
    for r=1:8
        at[r, i] = dot(ge[r, i], dm)
        @printf("r=%d,  <J x, y> : %2.5e, <x, J' y> : %2.5e, relative error : %2.5e \n",
                2^r, J0, at[r, i], (J0 - at[r, i])/(J0 + at[r, i]))
    end
end

att1 = [abs(1 - J0/at[i, 1]) for i=1:8]
att2 = [abs(1 - J0/at[i, 2]) for i=1:8]
figure()
semilogx([2^i for i=1:8], att1, basex=2, label="Probed adjoint test")
semilogx([2^i for i=1:8], att2, basex=2, label="Probed adjoint test reflections")
semilogx([2^i for i=1:8], [0 for i=1:8], label="target")
title(L"$1 - \frac{<J \delta m, g_e>}{<J' \delta d, \delta m>}$")
wsave(plotsdir("overthrust_single", "adjtest.png"), gcf())

# Probing vectors
figure();imshow(dobs.data[1]*dobs.data[1]', cmap="seismic", vmin=-10, vmax=10)
title(L"$d_{obs} d_{obs}^\top$")

figure()
for r=1:3:7
    subplot(3,3,r)
    Q = qr_data(dobs.data[1], 2^(r+2))
    imshow(Q, vmin=-.1, vmax=.1, cmap="seismic", aspect="auto", interpolation="none")
    title(L"$\mathbf{Z}$ "*"r=$(2^r)")
    subplot(3, 3, r+1)
    imshow(Q'*Q,vmin=-1e-2, vmax=1e-2, cmap="seismic", aspect="auto")
    title(L"$\mathbf{Z}^\top \mathbf{Z}$")
    subplot(3, 3, r+2)
    imshow(Q*Q',vmin=-1e-2, vmax=1e-2, cmap="seismic", aspect="auto")
    title(L"$\mathbf{Z}\mathbf{Z}^\top$")
end
tight_layout()
wsave(plotsdir("overthrust_single", "Zi.png"), gcf())

ext: jl

max_stars_repo_name: slimgroup/TimeProbeSeismic

max_issues_repo_name: slimgroup/TimeProbeSeismic

max_forks_repo_name: slimgroup/TimeProbeSeismic


text:
# This file is part of JuliaFMI.
# Licensed under MIT: https://github.com/AnHeuermann/JuliaFMI/blob/master/LICENSE.txt

# This file contains function to parse the modelDecription.xml of a FMU.

using LightXML

"""
Dynamicaly create symbols from array of EnumerationItemAttribute
"""
function createEnumMacro(enumerationItemAttribute::Array{EnumerationItemAttribute,1})

    enumNames = Set([Symbol(item.name) for item in enumerationItemAttribute])
    enumValue = Set([item.value for item in enumerationItemAttribute])

    # ToDo: Doing nothing at the moment
    #print
end

"""
Get typeDefinitions of fmiModelDescription.
Returns Array{SimpleType} with typeDefinitions
"""
function getTypeDefinitions(xroot::XMLElement)

    elementTypeDefinitions = find_element(xroot, "TypeDefinitions")
    if elementTypeDefinitions !== nothing
        # Loop over tag <SimpleType name="name" description="description">
        num_typeDefinitions = 0
        for element in child_elements(elementTypeDefinitions)
            num_typeDefinitions += 1
        end
        tmp_simpleTypes = Array{SimpleType}(undef,num_typeDefinitions)
        for (index,element) in enumerate(child_elements(elementTypeDefinitions))
            # ToDo AHeu: Add stuff
            tmp_name = attribute(element, "name"; required=true)
            tmp_description = attribute(element, "description"; required=false)

            # Loop over children of tag <SimpleType>
            tmp_attributes = nothing
            for children in child_nodes(element)
                if is_elementnode(children)
                    elementAttributeType = XMLElement(children)

                    # Attribute is Enumeration
                    if name(elementAttributeType) == "Enumeration"
                        # Check quantity
                        tmp_quantity = attribute(elementAttributeType, "quantity"; required=false)
                        if tmp_quantity === nothing
                            tmp_quantity = ""
                        end

                        # Count number of items
                        num_items = 0
                        for enumElem in child_elements(elementAttributeType)
                            num_items += 1
                        end
                        tmp_items = Array{EnumerationItemAttribute}(undef, num_items)

                        # Loop over <Item ...> tags of <Enumeration>
                        for (index, enumElem) in enumerate(child_elements(elementAttributeType))
                            tmp_attr_name = attribute(enumElem, "name"; required=true)
                            tmp_attr_value = parse(Int64, attribute(enumElem, "value"; required=true))
                            tmp_attr_description = attribute(enumElem, "description"; required=false)
                            if tmp_attr_description === nothing
                                tmp_attr_description = ""
                            end
                            tmp_items[index] = EnumerationItemAttribute(tmp_attr_name, tmp_attr_value, tmp_attr_description)
                        end
                        tmp_attributes = EnumerationAttributes(tmp_quantity, tmp_items)

                        # ToDo: Create Julia enum to use in EnumerationVariables
                        # createEnumMacro(tmp_items)

                    # ToDo: Add Real attributes, Integer attributes, Boolean attributes, String attributes
                    elseif name(elementAttributeType) == "Real"
                        @warn("SimpleType \"$(name(elementAttributeType))\" not implemented")
                        return nothing
                    elseif name(elementAttributeType) == "Integer"
                        @warn("SimpleType \"$(name(elementAttributeType))\" not implemented")
                        return nothing
                    elseif name(elementAttributeType) == "Boolean"
                        @warn("SimpleType \"$(name(elementAttributeType))\" not implemented")
                        return nothing
                    elseif name(elementAttributeType) == "String"
                        @warn("SimpleType \"$(name(elementAttributeType))\" not implemented")
                        return nothing

                    # Attribute is unknown
                    else
                        error("Parsing of modelDescription.xml failed: SimpleType \"$(name(elementAttributeType))\" unknown")
                    end
                end
            end

            if tmp_description === nothing
                tmp_simpleTypes[index] = SimpleType(tmp_name, "", tmp_attributes)
            else
                tmp_simpleTypes[index] = SimpleType(tmp_name, tmp_description, tmp_attributes)
            end
        end

        return tmp_simpleTypes
    # No TypeDefinitions found in XML
    else
        return nothing
    end
end

"""
Get log categories from fmiModelDescription.
Returns Array{LogCategory} with logCategories if found, otherwise nothing.
"""
function getLogCategories(xroot::XMLElement)

    elementLogCategories = find_element(xroot, "LogCategories")
    if elementLogCategories !== nothing
        numCategories = 0
        for element in child_elements(elementLogCategories)
            numCategories += 1
        end
        logCategories = Array{LogCategory}(undef, numCategories)

        for (index, element) in enumerate(child_elements(elementLogCategories))
            tmp_name = attribute(element, "name"; required=true)
            tmp_description = attribute(element, "description"; required=false)
            if tmp_description === nothing
                logCategories[index] = LogCategory(tmp_name)
            else
                logCategories[index] = LogCategory(tmp_name, tmp_description)
            end
        end
        return logCategories

    # No LogCategories found
    else
        return nothing
    end
end

"""
Get default experiment from fmiModelDescription.
Returns ExperimentData.
"""
function getDefaultExperiment(xroot::XMLElement)
    elementDefaultExperiment = find_element(xroot, "DefaultExperiment")
    if elementDefaultExperiment !== nothing
        startTime = attribute(elementDefaultExperiment, "startTime"; required=false)
        if startTime !== nothing
            startTime = parse(Float64, startTime)
        else
            startTime = 0
        end

        stopTime = attribute(elementDefaultExperiment, "stopTime"; required=false)
        if startTime !== nothing
            stopTime = parse(Float64, stopTime)
        else
            stopTime = startTime+1
        end

        tolerance = attribute(elementDefaultExperiment, "tolerance"; required=false)
        if tolerance !== nothing
            tolerance = parse(Float64, tolerance)
        else
            tolerance = 1e-6
        end

        stepSize = attribute(elementDefaultExperiment, "stepSize"; required=false)
        if stepSize !== nothing
            stepSize = parse(Float64, stepSize)
        else
            stepSize = 2e-3
        end

        defaultExperiment = ExperimentData(startTime, stopTime, tolerance, stepSize)
    else
        defaultExperiment = ExperimentData()
    end

    return defaultExperiment
end

"""
Get model variables from fmiModelDescription.
Returns Array{ScalarVariable}.
"""
function getModelVariables(xroot::XMLElement)

    elementModelVariables = find_element(xroot, "ModelVariables")
    if elementModelVariables === nothing
        error("modelDescription.xml is missing ModelVariables tag")
    end

    numberOfVariables = 0
    for element in get_elements_by_tagname(elementModelVariables, "ScalarVariable")
        numberOfVariables += 1
    end
    scalarVariables = Array{ScalarVariable}(undef, numberOfVariables)

    for (index, element) in enumerate(get_elements_by_tagname(elementModelVariables, "ScalarVariable"))
        # Get attributes name, valueReference, variability, causality, initial
        tmp_name = attribute(element, "name"; required=true)
        tmp_valueReference = parse(Int, attribute(element, "valueReference"; required=true))

        tmp_description = attribute(element, "description"; required=false)
        if tmp_description === nothing
            tmp_description=""
        end
        tmp_variability = attribute(element, "variability"; required=false)
        if tmp_variability === nothing
            tmp_variability=""
        end
        tmp_causality = attribute(element, "causality"; required=false)
        if tmp_causality === nothing
            tmp_causality=""
        end
        tmp_initial = attribute(element, "initial"; required=false)
        if tmp_initial === nothing
            tmp_initial=""
        end
        tmp_canHandleMultipleSetPerTimelnstant = attribute(element, "canHandleMultipleSetPerTimelnstant"; required=false)
        if tmp_canHandleMultipleSetPerTimelnstant === nothing
            tmp_canHandleMultipleSetPerTimelnstant = false
        else
            tmp_canHandleMultipleSetPerTimelnstant = parse(Bool,
                tmp_canHandleMultipleSetPerTimelnstant)
        end

        # Get child node for typeSpecificProperties
        tmp_typeSpecificProperties = nothing
        for child in child_elements(element)
            if is_elementnode(child)
                if name(child)=="Real"
                    tmp_declaredType = "Real"
                    tmp_variableAttributes = RealAttributes()   # TODO implement
                    tmp_start = attribute(child, "start"; required=false)
                    if tmp_start === nothing
                        tmp_start = Float64(0)
                    else
                        tmp_start = parse(Float64, tmp_start)
                    end
                    tmp_derivative = attribute(child, "derivative"; required=false)
                    if tmp_derivative === nothing
                        tmp_derivative = UInt(0)
                    else
                        tmp_derivative = parse(UInt, tmp_derivative)
                    end
                    tmp_reinit = attribute(child, "reinit"; required=false)
                    if tmp_reinit === nothing
                        tmp_reinit = false
                    end
                    tmp_typeSpecificProperties = RealProperties(tmp_declaredType, tmp_variableAttributes, tmp_start, tmp_derivative, tmp_reinit)
                elseif name(child)=="Integer"
                    tmp_declaredType = "Integer"
                    tmp_variableAttributes = IntegerAttributes()   # TODO implement
                    tmp_start = attribute(child, "start"; required=false)
                    if tmp_start === nothing
                        tmp_start = Int(0)
                    else
                        tmp_start = parse(Int, tmp_start)
                    end
                    tmp_typeSpecificProperties = IntegerProperties(tmp_declaredType, tmp_variableAttributes, tmp_start)
                elseif name(child)=="Boolean"
                    tmp_declaredType = "Boolean"
                    tmp_start = attribute(child, "start"; required=false)
                    if tmp_start === nothing
                        tmp_start = false
                    else
                        tmp_start = parse(Bool, tmp_start)
                    end
                    tmp_typeSpecificProperties = BooleanProperties(tmp_declaredType, tmp_start)
                elseif name(child)=="String"
                    tmp_declaredType = "String"
                    tmp_start = attribute(child, "start"; required=false)
                    if tmp_start === nothing
                        tmp_start = ""
                    end
                    tmp_typeSpecificProperties = StringProperties(tmp_declaredType, tmp_start)
                elseif name(child)=="Enumeration"
                    tmp_declaredType = "Enumeration"
                    tmp_quantity = attribute(child, "quantity"; required=false)
                    if tmp_quantity === nothing
                        tmp_quantity = ""
                    end
                    tmp_min = attribute(child, "min"; required=false)
                    if tmp_min === nothing
                        tmp_min = Int(0)
                    else
                        tmp_min = parse(Int, tmp_min)
                    end
                    tmp_max = attribute(child, "max"; required=false)
                    if tmp_max === nothing
                        tmp_max = Int(0)
                    else
                        tmp_max = parse(Int, tmp_max)
                    end
                    tmp_start = attribute(child, "start"; required=false)
                    if tmp_start === nothing
                        tmp_start = Int(0)
                    else
                        tmp_start = parse(Int, tmp_start)
                    end
                    tmp_typeSpecificProperties = EnumerationProperties(tmp_declaredType, tmp_quantity, tmp_min, tmp_max, tmp_start)
                else
                    error("Unknown type \"$(name(child))\" of ScalarVariable")
                end
            end
        end

        scalarVariables[index] = ScalarVariable(tmp_name,
            tmp_valueReference, tmp_description, tmp_causality,
            tmp_variability, tmp_initial,
            tmp_canHandleMultipleSetPerTimelnstant,
            tmp_typeSpecificProperties)
    end

    return scalarVariables
end

"""
Parse modelDescription.xml
"""
function readModelDescription(pathToModelDescription::String)

    md = ModelDescription()

    if !isfile(pathToModelDescription)
        error("File $pathToModelDescription does not exist.")
    elseif basename(pathToModelDescription) != "modelDescription.xml"
        error("File name is not equal to \"modelDescription.xml\" but $(basename(pathToModelDescription))" )
    end

    # Parse modelDescription
    xdoc = parse_file(pathToModelDescription)

    # Check root tag
    xroot = root(xdoc)
    if name(xroot)!="fmiModelDescription"
        error("modelDescription.xml root is not \"fmiModelDescription\".")
    end

    # Get attributes of tag <fmiModelDescription>
    attributes = attributes_dict(xroot)
    try
        md.fmiVersion = attributes["fmiVersion"]
        md.modelName = attributes["modelName"]
        md.guid = attributes["guid"]

        md.description = get(attributes, "description", "")
        md.author = get(attributes, "author", "")
        md.version = get(attributes, "version", "")
        md.copyright = get(attributes, "copyright", "")
        md.license = get(attributes, "license", "")
        md.generationTool = get(attributes, "generationTool", "")
        md.generationDateAndTime = get(attributes, "generationDateAndTime", "")
        md.variableNamingConvention = str2NamingConvention(get(attributes, "variableNamingConvention", "flat"))
        md.numberOfEventIndicators = parse(Int, attributes["numberOfEventIndicators"])

        # Get attributes of tag <ModelExchange>
        elementModelExchange = find_element(xroot, "ModelExchange")

        # Get attributes of tag <CoSimulation>
        elementCoSimulation = find_element(xroot, "CoSimulation")

        try
            if elementModelExchange !== nothing
                md.isModelExchange = true
                md.modelIdentifier = attribute(elementModelExchange, "modelIdentifier"; required=true)
            else
                md.isModelExchange = false
                error("While parsing modelDescription: Only ModelExchange is supported yet but
                    FMU does not support ModelExchange.")
            end

            if elementCoSimulation !== nothing
                md.isCoSimulation = true
                md.modelIdentifier = attribute(elementModelExchange, "modelIdentifier"; required=true)
            else
                md.isCoSimulation = false
            end
        catch err
            if isa(err, LightXML.XMLAttributeNotFound)
                error("While parsing modelDescription: Non-optinal element \"modelIdentifier\" not found")
            else
                rethrow(err)
            end
        end

        if elementCoSimulation === nothing && elementModelExchange === nothing
            error("modelDescription.xml is missing ModelExchange and CoSimulation tags.")
        end

        # ToDo: Add stuff for tag UnitDefinitions

        # Get attributes of tag TypeDefinitions
        md.typeDefinitions = getTypeDefinitions(xroot)

        # Get attributes of tag LogCategories
        md.logCategories = getLogCategories(xroot)

        # Get attributes of tag DefaultExperiment
        md.defaultExperiment = getDefaultExperiment(xroot)

        # ToDo: Add stuff for tag VendorAnnotations

        # Get attributes of tag ModelVariables
        md.modelVariables = getModelVariables(xroot)

        # Get attributes of tag ModelStructure

    catch err
        if isa(err, KeyError)
            error("While parsing modelDescription: Non-optional element \"$(err.key)\" not found")
        else
            rethrow(err)
        end
    end

    # Free memory
    LightXML.free(xdoc)

    return md
end

max_stars_repo_name: AnHeuermann/JuliaFMU

max_issues_repo_name: AnHeuermann/JuliaFMU

max_forks_repo_name: jwscook/JuliaFMI


text:
@testset " spline pp 1d " begin
    import GEMPIC: SplinePP, b_to_pp, uniform_bsplines_eval_basis, horner_1d

    ncells = 8

    domain = [0.0, 2π]
    delta_x = (domain[2] - domain[1]) / ncells

    b = rand(ncells)

    for d in 1:5
        xp = 4.8141437173987800

        spline_pp = SplinePP(d, ncells)

        pp = b_to_pp(spline_pp, ncells, b)

        xi = (xp - domain[1]) / delta_x
        index = floor(Int64, xi) + 1
        xi = xi - (index - 1)
        res1 = horner_1d(d, pp, xi, index)

        index = index - d

        val = uniform_bsplines_eval_basis(d, xi)

        res2 = 0.0

        for i in 1:(d + 1)
            index1d = (index + i - 2) % ncells + 1
            res2 += b[index1d] * val[i]
        end

        @test res1 ≈ res2

        @test spline_pp.degree ≈ d

        xp = rand()
        index = 1
        res1 = horner_1d(d, pp, xp, index)

        res2 = 0.0
        for i in 1:(d + 1)
            res2 += pp[i, 1] * xp^((d + 1) - i)
        end

        @test res1 ≈ res2
    end
end

@testset " spline pp 2d " begin
    using Random

    ncells = 50
    d = 3

    b = zeros(ncells * ncells)
    pp = zeros((d + 1) * (d + 1), ncells * ncells)

    dx = 2π / ncells
    dy = 2π / ncells

    rng = MersenneTwister(42)
    rand!(rng, b)

    spline1 = SplinePP(d, ncells)
    spline2 = SplinePP(d, ncells)

    GEMPIC.b_to_pp_2d!(pp, spline1, spline2, b)

    xp, yp = rand(rng, 2) .* 2π

    xi = xp / dx
    yi = yp / dy

    ind_x = floor(Int, xi) + 1
    ind_y = floor(Int, yi) + 1

    xi -= (ind_x - 1)
    yi -= (ind_y - 1)

    res1 = GEMPIC.horner_2d((d, d), pp, (xi, yi), (ind_x, ind_y), (ncells, ncells))

    ind_x -= d
    ind_y -= d

    val1 = uniform_bsplines_eval_basis(d, xi)
    val2 = uniform_bsplines_eval_basis(d, yi)

    res2 = 0.0
    for i in 1:(d + 1)
        idx1 = mod(ind_x + i - 2, ncells)
        for j in 1:(d + 1)
            idx2 = mod(ind_y + j - 2, ncells)
            index2d = idx1 + idx2 * ncells + 1
            res2 += b[index2d] * val1[i] * val2[j]
        end
    end

    @test res1 ≈ res2

    xp = rand(rng, 2)

    res1 = GEMPIC.horner_2d((d, d), pp, xp, [1, 1], [1, 1])

    res2 = 0.0
    for i in 1:(d + 1), j in 1:(d + 1)
        res2 += pp[i + (j - 1) * (d + 1), 1] * xp[1]^((d + 1) - i) * xp[2]^((d + 1) - j)
    end

    @test res1 ≈ res2
end


text:
"""
    Ai2

Ai2 performs over-approximated reachability analysis to compute the over-approximated output reachable set for a network.

# Problem requirement
1. Network: any depth, ReLU activation (more activations to be supported in the future)
2. Input: HPolytope
3. Output: HPolytope

# Return
`ReachabilityResult`

# Method
Reachability analysis using split and join.

# Property
Sound but not complete.

# Reference
T. Gehr, M. Mirman, D. Drashsler-Cohen, P. Tsankov, S. Chaudhuri, and M. Vechev,
"Ai2: Safety and Robustness Certification of Neural Networks with Abstract Interpretation,"
in *2018 IEEE Symposium on Security and Privacy (SP)*, 2018.
"""
struct Ai2 end

function solve(solver::Ai2, problem::Problem)
    reach = forward_network(solver, problem.network, problem.input)
    return check_inclusion(reach, problem.output)
end

forward_layer(solver::Ai2, layer::Layer, inputs::Vector{<:AbstractPolytope}) = forward_layer.(solver, layer, inputs)

function forward_layer(solver::Ai2, layer::Layer, input::AbstractPolytope)
    W, b, act = layer.weights, layer.bias, layer.activation
    outLinear = shiftcenter(linear_map(W, input), b)
    return transform(act, outLinear)
end

function transform(f::ReLU, P::AbstractPolytope)
    polys = VPolytope[]
    # check if 1:2^n relus is tighter range than relu(relu(...to the n))
    for i in dim(P)
        pos = meet(P, i, true)
        neg = meet(P, i, false)
        relu_neg = VPolytope([f(v) for v in vertices_list(neg)]) #dimensional relu

        push!(polys, tovrep(pos))
        push!(polys, relu_neg)
    end

    hull = polys[1]
    for P in polys[2:end]
        hull = convex_hull(hull, P)
    end

    return hull
end

# constraints are added in H-representation
meet(V::VPolytope, pos::Bool) = meet(tohrep(V), pos)
function meet(HP::HPolytope{T}, pos::Bool) where T
    H = copy(HP)
    # constraints are given by ax <= b so (-) is required for a positive constraint
    if (pos)  d = Matrix(-1.0I, dim(H), dim(H))
    else      d = Matrix(1.0I, dim(H), dim(H))
    end

    for i in size(d, 1)
        new_hs = LazySets.HalfSpace(d[i, :], zero(T))
        addconstraint!(H, new_hs)
    end
end

shiftcenter(zono::Zonotope, shift::Vector)         = Zonotope(zono.center + shift, zono.generators)
shiftcenter(poly::AbstractPolytope, shift::Vector) = shiftcenter(tovrep(poly), shift)

function shiftcenter(V::VPolytope, shift::Vector)
    shifted = [v + shift for v in vertices_list(V)]
    return VPolytope(shifted)
end

#=
meet and join will still become necessary in the zonotope case
The Case type is probably not necessary for correct dispatch
=#
# function meet(case::Case, H::HPolytope)
#     addconstraint!(H, constraint(case, H))
#     return H
# end

# function constraint(case::Type{Case}, n_dims, constraint_dim)
#     space = zeros(n_dims)
#     if case == Pos
#         space[constraint_dim] =  1.0
#     elseif case == Neg
#         space[constraint_dim] = -1.0
#     end
#     return HalfSpace(space, 0.0)
# end

# constraint(case::Case, poly::AbstractPolytope) = constraint(typeof(case), dim((poly), case.i)


text:
# This file is a part of AstroLib.jl. License is MIT "Expat".
# Copyright (C) 2016 Mosè Giordano.

"""
    cirrange(number[, max]) -> restricted_number

### Purpose ###

Force a number into a given range \$[0, \\text{max})\$.

### Argument ###

* `number`: the number to modify.  Can be a scalar or an array.
* `max` (optional numerical argument): specify the extremum of the range \$[0, \\text{max})\$
 into which the number should be restricted.  If omitted, defaults to `360.0`.

### Output ###

The converted number or array of numbers, as `AbstractFloat`.

### Example ###

Restrict an array of numbers in the range \$[0, 2\\pi)\$ as if they are angles
expressed in radians:

``` julia
cirrange([4pi, 10, -5.23], 2.0*pi)
# => 3-element Array{Float64,1}:
#     0.0
#     3.71681
#     1.05319
```

### Notes ###

This function does not support the `radians` keyword like IDL implementation.
Use `2.0*pi` as second argument to restrict a number to the same interval.
"""
cirrange(number::Real, max::Real=360.0) =
    mod(promote(float(number), float(max))...)

function cirrange{N<:Real}(numbers::AbstractArray{N}, max::Real=360.0)
    result = similar(numbers, typeof(float(one(N))))
    for i in eachindex(numbers)
        result[i] = cirrange(numbers[i], max)
    end
    return result
end


text:
"""
cssls!(AtA,AtB,X,P)

Solve combinatorial subspace least squares (CSSLS) problem. Find X[P] that
minimizes ||A*X[P] - B[P]|| in the least squares sense, where P is a
1-bit matrix encoding the combinatorial subspace. Input X is overwritten
with the solution.

In the context of nonnegative least squares, each column of X has a passive
set which is encoded by "true" entries in P, and an active set which is encoded
by "false" entries in P.

Inputs:
	AtA = A' * A = (n x n) matrix
	AtB = A' * B = (n x k)
	P = 1-bit matrix of Bools = (? x ?) matrix

References:
	J. Kim and H. Park, Fast nonnegative matrix factorization: An
	active-set-like method and comparisons, SIAM J. Sci. Comput., 33 (2011),
	pp. 3261–3281.

	M. H. Van Benthem and M. R. Keenan, Fast algorithm for the solution of
	large-scale non-negativity-constrained least squares problems. J.
	Chemometrics 2004; 18: 441-450
"""
function cssls!(
	AtA::Matrix{Float64}, # (n x n) matrix
	AtB::Matrix{Float64}, # (n x p) matrix
	X::Matrix{Float64},   # (n x p) matrix
	P::AbstractArray{Bool}
	)

	n,p = size(AtB)

	# Find unique columns in P
	U = unique(P,dims=2)
	num_unique = size(U,2)

	# Find indices associates with unique columns in P
	E = (Array{Int})[]
	rp = collect(1:p)
	for i = 1:num_unique
		# array of indices where P[:,e] == U[:,i]
		e = (Int)[]

		# find columns of P that match U[:,i]
		j = 1
		while j <= length(rp)
			# rp contains columns of P that haven't been assigned
			t = rp[j]

			# if column t matches U[:,i] store it, and delete it from rp (don't
			# revisit preiously assigned columns)
			if all(P[:,t] .== U[:,i])
				push!(e,t)      # store column t
				deleteat!(rp,j) # delete t from rp (stored at position j)
			else
				# column t didn't match, move to next column
				j += 1
			end
		end

		# store the set of columns matching U[:,i]
		push!(E,e)
	end

	# Update X, solve each unique combinatorial subspace
	for i = 1:num_unique
		rr = U[:,i] # row mask (the passive set)

		# If the passive set is empty, these columns are already feasible.
		# So don't waste time updating them.
		if any(rr)
			cc = E[i]   # column mask (columns of P matching U[:,i])
			X[rr,cc] = pinv(AtA[rr,rr])*AtB[rr,cc]
		end
	end
end


text:
"""
    Stacked(bs)
    Stacked(bs, ranges)
    stack(bs::Bijector{0}...) # where `0` means 0-dim `Bijector`

A `Bijector` which stacks bijectors together which can then be applied to a vector
where `bs[i]::Bijector` is applied to `x[ranges[i]]::UnitRange{Int}`.

# Arguments
- `bs` can be either a `Tuple` or an `AbstractArray` of 0- and/or 1-dimensional bijectors
  - If `bs` is a `Tuple`, implementations are type-stable using generated functions
  - If `bs` is an `AbstractArray`, implementations are _not_ type-stable and use iterative methods
- `ranges` needs to be an iterable consisting of `UnitRange{Int}`
  - `length(bs) == length(ranges)` needs to be true.

# Examples
```
b1 = Logit(0.0, 1.0)
b2 = Identity{0}()
b = stack(b1, b2)
b([0.0, 1.0]) == [b1(0.0), 1.0]  # => true
```
"""
struct Stacked{Bs, N} <: Bijector{1}
    bs::Bs
    ranges::NTuple{N, UnitRange{Int}}

    function Stacked(
        bs::C,
        ranges::NTuple{N, UnitRange{Int}}
    ) where {N, C<:Tuple{Vararg{<:ZeroOrOneDimBijector, N}}}
        return new{C, N}(bs, ranges)
    end

    function Stacked(
        bs::A,
        ranges::NTuple{N, UnitRange{Int}}
    ) where {N, A<:AbstractArray{<:Bijector}}
        @assert length(bs) == N "number of bijectors is not same as number of ranges"
        @assert all(b -> isa(b, ZeroOrOneDimBijector), bs)
        return new{A, N}(bs, ranges)
    end
end
Stacked(bs, ranges::AbstractArray) = Stacked(bs, tuple(ranges...))
Stacked(bs) = Stacked(bs, tuple([i:i for i = 1:length(bs)]...))

function Base.:(==)(b1::Stacked, b2::Stacked)
    bs1, bs2 = b1.bs, b2.bs
    if !(bs1 isa Tuple && bs2 isa Tuple || bs1 isa Vector && bs2 isa Vector)
        return false
    end
    return all(bs1 .== bs2) && all(b1.ranges .== b2.ranges)
end

isclosedform(b::Stacked) = all(isclosedform, b.bs)

stack(bs::Bijector{0}...) = Stacked(bs)

# For some reason `inv.(sb.bs)` was unstable... This works though.
inv(sb::Stacked) = Stacked(map(inv, sb.bs), sb.ranges)
# map is not type stable for many stacked bijectors as a large tuple
# hence the generated function
@generated function inv(sb::Stacked{A}) where {A <: Tuple}
    exprs = []
    for i = 1:length(A.parameters)
        push!(exprs, :(inv(sb.bs[$i])))
    end
    :(Stacked(($(exprs...), ), sb.ranges))
end

@generated function _transform(x, rs::NTuple{N, UnitRange{Int}}, bs::Bijector...) where N
    exprs = []
    for i = 1:N
        push!(exprs, :(bs[$i](x[rs[$i]])))
    end
    return :(vcat($(exprs...)))
end
function _transform(x, rs::NTuple{1, UnitRange{Int}}, b::Bijector)
    @assert rs[1] == 1:length(x)
    return b(x)
end
function (sb::Stacked{<:Tuple})(x::AbstractVector{<:Real})
    y = _transform(x, sb.ranges, sb.bs...)
    @assert size(y) == size(x) "x is size $(size(x)) but y is $(size(y))"
    return y
end
# The Stacked{<:AbstractArray} version is not TrackedArray friendly
function (sb::Stacked{<:AbstractArray, N})(x::AbstractVector{<:Real}) where {N}
    y = mapvcat(1:N) do i
        sb.bs[i](x[sb.ranges[i]])
    end
    @assert size(y) == size(x) "x is size $(size(x)) but y is $(size(y))"
    return y
end

(sb::Stacked)(x::AbstractMatrix{<:Real}) = eachcolmaphcat(sb, x)
function logabsdetjac(
    b::Stacked{<:Any, N},
    x::AbstractVector{<:Real}
) where {N}
    init = sum(logabsdetjac(b.bs[1], x[b.ranges[1]]))
    init + sum(2:N) do i
        sum(logabsdetjac(b.bs[i], x[b.ranges[i]]))
    end
end

function logabsdetjac(b::Stacked{<:Any, 1}, x::AbstractVector{<:Real})
    return sum(logabsdetjac(b.bs[1], x[b.ranges[1]]))
end

function logabsdetjac(b::Stacked, x::AbstractMatrix{<:Real})
    return map(eachcol(x)) do c
        logabsdetjac(b, c)
    end
end

# Generates something similar to:
#
# quote
#     (y_1, _logjac) = forward(b.bs[1], x[b.ranges[1]])
#     logjac = sum(_logjac)
#     (y_2, _logjac) = forward(b.bs[2], x[b.ranges[2]])
#     logjac += sum(_logjac)
#     return (rv = vcat(y_1, y_2), logabsdetjac = logjac)
# end
@generated function forward(b::Stacked{T, N}, x::AbstractVector) where {N, T<:Tuple}
    expr = Expr(:block)
    y_names = []

    push!(expr.args, :((y_1, _logjac) = forward(b.bs[1], x[b.ranges[1]])))
    # TODO: drop the `sum` when we have dimensionality
    push!(expr.args, :(logjac = sum(_logjac)))
    push!(y_names, :y_1)
    for i = 2:length(T.parameters)
        y_name = Symbol("y_$i")
        push!(expr.args, :(($y_name, _logjac) = forward(b.bs[$i], x[b.ranges[$i]])))

        # TODO: drop the `sum` when we have dimensionality
        push!(expr.args, :(logjac += sum(_logjac)))

        push!(y_names, y_name)
    end

    push!(expr.args, :(return (rv = vcat($(y_names...)), logabsdetjac = logjac)))
    return expr
end

function forward(sb::Stacked{<:AbstractArray, N}, x::AbstractVector) where {N}
    yinit, linit = forward(sb.bs[1], x[sb.ranges[1]])
    logjac = sum(linit)
    ys = mapvcat(drop(sb.bs, 1), drop(sb.ranges, 1)) do b, r
        y, l = forward(b, x[r])
        logjac += sum(l)
        y
    end
    return (rv = vcat(yinit, ys), logabsdetjac = logjac)
end


text:
### A Pluto.jl notebook ###
# v0.12.18

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ 2069c2c0-01b4-11eb-3cc9-a7c60467a92a
# Load packages
begin
	using Pkg
	Pkg.activate(mktempdir())
	Pkg.add("Plots")
	Pkg.develop(path="/home/sari/.julia/dev/FractalTools/")
	using Plots
	plotly()
	using FractalTools
end

# ╔═╡ 24b8a340-01b5-11eb-0551-318932fe4a10
@bind d html"<input type=range min=0 max=1 step=1e-3>"

# ╔═╡ 2caf47f8-047f-11eb-1f15-db8e661dc9ce
@bind niter html"<input type=range min=1 max=30 step=1>"

# ╔═╡ 69dbf05e-047f-11eb-3d6e-892350bec24f
niter, d

# ╔═╡ 402f7b62-01b5-11eb-2789-7bdb04c0d694
begin
	# Consrucct data
	f(x) = sin(2π*x)
	g(x) = cos(2π*x)
	xi, dx, xf = 0, 0.1, 1
	x = collect(xi : dx : xf)
	y = f.(x)
	z = g.(x)
	dvar = d * ones(length(x) - 1)
	h = d * ones(length(x) - 1)
	l = d * ones(length(x) - 1)
	m = d * ones(length(x) - 1)

	# Interplate
	interp = hiddenfif(x, y, z, d=dvar, h=h, l=l, m=m, niter=niter)

	# Plots
	plt = plot()
	xd = collect(xi : 0.1dx : xf)
	yd = interp.(xd)
	plot!(xd, getindex.(yd, 1))
	scatter!(x, y, marker=(:circle, 3))
end

# ╔═╡ Cell order:
# ╠═2069c2c0-01b4-11eb-3cc9-a7c60467a92a
# ╠═24b8a340-01b5-11eb-0551-318932fe4a10
# ╠═2caf47f8-047f-11eb-1f15-db8e661dc9ce
# ╠═69dbf05e-047f-11eb-3d6e-892350bec24f
# ╠═402f7b62-01b5-11eb-2789-7bdb04c0d694


text:
# -*- coding: utf-8 -*-
# ---
# jupyter:
#   jupytext:
#     formats: jl:light,ipynb
#     text_representation:
#       extension: .jl
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.5.1
#   kernelspec:
#     display_name: Julia 1.4.2
#     language: julia
#     name: julia-1.4
# ---

# +
"""Tutorial: Second-Order Moller--Plesset Perturbation Theory (MP2)"""

__author__    = ["D. Menendez", "Dominic A. Sirianni"]
__credit__    = ["D. Menendez", "Dominic A. Sirianni", "Daniel G. A. Smith"]

__copyright__ = "(c) 2014-2020, The Psi4Julia Developers"
__license__   = "BSD-3-Clause"
__date__      = "2020-07-30"
# -

# # Second-Order Moller-Plesset Perturbation Theory (MP2)
#
# Moller-Plesset perturbation theory [also referred to as many-body perturbation theory (MBPT)] is an adaptation of the more general Rayleigh-Schrodinger perturbation theory (RSPT), applied to problems in molecular electronic structure theory.  This tutorial will provide a brief overview of both RSPT and MBPT, before walking through an implementation of second-order Moller-Plesset perturbation theory (specifically referred to as MP2) which uses conventional, 4-index ERIs.
#
# ### I. Overview of Rayleigh-Schrodinger Perturbation Theory
# Given the Hamiltonian operator $\hat{H}$ for a system, perturbation theory solves the Schrodinger equation for that system by rewriting $\hat{H}$ as
#
# \begin{equation}
# \hat{H} = \hat{H}{}^{(0)} + \lambda\hat{V},
# \tag{[Szabo:1996], pp. 322, Eqn. 6.3}
# \end{equation}
#
# were $\hat{H}{}^{(0)}$ is the Hamiltonian operator corresponding to a solved problem which resembles $\hat{H}$, and $\hat{V}$ is the *perturbation* operator, defined as $\hat{V} = \hat{H} - \hat{H}{}^{(0)}$.  Then the Schrodinger equation for the system becomes
#
# \begin{equation}
# \hat{H}\mid\Psi_n\rangle = (\hat{H}{}^{(0)} + \lambda\hat{V})\mid\Psi_n\rangle = E_n\mid\Psi_n\rangle.
# \tag{[Szabo:1996], pp. 322, Eqn. 6.2}
# \end{equation}
#
# The energies $E_n$ and wavefunctions $\mid\Psi_n\rangle$ will both be functions of $\lambda$; they can therefore be written as a Taylor series expansion about $\lambda = 0$ ([Szabo:1996], pp. 322, Eqns. 6.4a & 6.4b):
#
# \begin{align}
# E_n &= E_n^{(0)} + \lambda E_n^{(1)} + \lambda^2E_n^{(2)} + \ldots;\tag{[Szabo:1996], pp. 322, Eqn. 6.4a}\\
# \mid\Psi_n\rangle &= \mid\Psi_n^{(0)}\rangle + \lambda\mid\Psi_n^{(1)}\rangle + \lambda^2\mid\Psi_n^{(2)}\rangle + \ldots,\tag{[Szabo:1996], pp. 322, Eqn. 6.4b}\\
# \end{align}
#
# in practice, these perturbation expansions may be truncated to a given power of $\lambda$.  Substituting the perturbation series above back into the Schrodinger equation yields
#
# \begin{equation*}
# (\hat{H}{}^{(0)} + \lambda\hat{V})(\mid\Psi_n^{(0)}\rangle + \lambda\mid\Psi_n^{(1)}\rangle + \lambda^2\mid\Psi_n^{(2)}\rangle + \ldots) = (E_n^{(0)} + \lambda E_n^{(1)} + \lambda^2E_n^{(2)} + \ldots)(\mid\Psi_n^{(0)}\rangle + \lambda\mid\Psi_n^{(1)}\rangle + \lambda^2\mid\Psi_n^{(2)}\rangle + \ldots),
# \end{equation*}
#
# which by equating powers of $\lambda$ ([Szabo:1996], pp. 323, Eqns. 6.7a-6.7d) gives expressions for the $E_n^{(i)}$and $\mid\Psi_n^{(i)}\rangle$. Note that for $\lambda^0$, $E_n^{(0)}$ and $\mid\Psi_n^{(0)}\rangle$ are known, as they are the solution to the zeroth-order problem $\hat{H}{}^{(0)}$.  For $\lambda^1$ and $\lambda^2$, the expressions for $E_n^{(1)}$ and $E_n^{(2)}$ are given by
#
# \begin{align}
# \lambda^1:\;\;\;\;E_n^{(1)} &= \langle\Psi_n^{(0)}\mid\hat{V}\mid\Psi_n^{(0)}\rangle,\tag{[Szabo:1996], pp. 323, Eqn. 6.8b}\\
# \lambda^2:\;\;\;\;E_n^{(2)} &= \sum_{\mu\neq n}\frac{\mid\langle\Psi_{n}^{(0)}\mid\hat{V}\mid\Psi_{\mu}^{(0)}\rangle\mid^2}{E_n^{(0)} - E_{\mu}^{(0)}}\tag{[Szabo:1996], pp. 324, Eqn. 6.12}\\
# \end{align}
#

# ### II. Overview of Moller-Plesset Perturbation Theory
#
# The exact electronic Hamiltonian for an N-electron molecule with a given atomic configuration is (in atomic units):
#
# \begin{equation}
# \hat{H}_{elec} = \sum_i\hat{h}(i) + \sum_{i<j}\frac{1}{r_{ij}}.\tag{[Szabo:1996], pp. 43, Eqn. 2.10}
# \end{equation}
#
# Moller-Plesset perturbation theory seeks to solve the molecular electronic Schrodinger equation with the above Hamiltonian using the techniques of Rayleigh-Schroding perturbation theory, by selecting the zeroth-order Hamiltonian and wavefunctions to be those from Hartree-Fock theory:
#
# \begin{align}
# \hat{H}{}^{(0)}  &= \sum_i \hat{f}(i) = \sum_i \hat{h}(i) + \upsilon^{HF}(i)\tag{[Szabo:1996], pp. 350, Eqn. 6.59}\\
# \hat{V} &= \hat{H} - \hat{H}{}^{(0)} = \sum_{i<j} \frac{1}{r_{ij}} - \upsilon^{HF}(i).\tag{[Szabo:1996], pp. 350, Eqn. 6.60}\\
# \end{align}
#
# With these choices of $\hat{H}{}^{(0)}$, $\hat{V}$, and $\mid\Psi_n^{(0)}\rangle$, the ground-state zeroth-order energy is given by $E_0^{(0)} = \sum_i\epsilon_i$ ([Szabo:1996], pp. 351, Eqn. 6.67).  Then, the first-order ground state energy may be computed by $E_0^{(1)} = \langle\Psi_0^{HF}\mid\hat{V}\mid\Psi_0^{HF}\rangle$ to find that the total ground-state energy through first order, $E_0 = E_0^{(0)} + E_0^{(1)}$, is exactly the Hartree-Fock energy. ([Szabo:1996], pp. 351, Eqn. 6.69)  Therefore, the first correction to the Hartree-Fock energy will occur at second-order in the perturbation series, i.e., with $E_0^{(2)}$; truncating the perturbation series at second order is commonly referred to as MP2.  The second order correction to the ground state energy will be given by
#
# \begin{equation}
# E_0^{(2)} = \sum_{\mu\neq 0}\frac{\left|\langle\Psi_{0}^{HF}\mid\hat{V}\mid\Psi_{\mu}^{HF}\rangle\right|^2}{E_0^{(0)} - E_{\mu}^{(0)}}\tag{[Szabo:1996], pp. 351, Eqn. 6.70}
# \end{equation}
#
# For brevity, we will now drop the "HF" from all zeroth-order wavefunctions.  This summation is over the eigenstate index $\mu$, each associated with a different eigenstate of the zeroth-order Hamiltonian.  For a single-determinant wavefunction constructed from spin orbitals, the summation over the eigenstate index $\mu\neq 0$ therefore refers to determinants which are constructed from *different* spin orbitals than the ground state determinant.  To distinguish such determinants, we will denote MOs occupied in the ground state with indices $i,\,j,\,\ldots$, and MOs which are unoccupied in the ground state with indices $a,\,b,\,\ldots\,$.  Then a determinant where orbital $a$ is substituted for orbital $i$ is denoted $\mid\Psi_i^a\rangle$, and so on.  Before substituting this new notation into the above energy expression, however, we may immediately recognize that many terms $\langle\Psi_{0}\mid\hat{V}\mid\Psi_{\mu}\rangle$ will not contribute to the second order energy:
#
# | Term         | Determinant                               |  Contribution to $E_0^{(2)}$                            |
# |--------------|-------------------------------------------|---------------------------------------------------------|
# | Singles      | $\mid\Psi_i^a\rangle$                     | 0; Brillouin's Theorem                                  |
# | Doubles      | $\mid\Psi_{ij}^{ab}\rangle$               | Survive                                                 |
# | Higher-order | $\mid\Psi_{ijk\ldots}^{abc\ldots}\rangle$ | 0; $\hat{V}$ is a two-particle operator            |
#
# Hence we see that only doubly-substituted determinants $\mid\Psi_{ij}^{ab}\rangle$ will contribute to $E_0^{(2)}$.  From Hartree-Fock theory, we know that
#
# \begin{equation}
# \langle\Psi_0\mid r_{ij}^{-1}\mid\Psi_{ij}^{ab}\rangle = [ia\| jb],\tag{[Szabo:1996], pp. 72, Tbl. 2.6}
# \end{equation}
#
# where $[ia\| jb] = [ia\mid jb] - [ib\mid ja]$ is an antisymmetrized two-electron integral, and the square brackets "$[\ldots ]$" indicate that we are employing chemists' notation.  What about the energies of these doubly substituted determinants?  Recognizing that the difference between the energies of the newly- and formerly-occupied orbitals in each substitution must modify the total energy of the ground state determinant,
#
# \begin{equation}
# E_{ij}^{ab} = E_0 - (\epsilon_i - \epsilon_a + \epsilon_j - \epsilon_b).
# \end{equation}
#
# Substituting these expressions into the one for the second-order energy, we have that
#
# \begin{equation}
# E_0^{(2)} = \sum_{i<j}\sum_{a<b} \frac{\left|\,[ia\|jb]\,\right|^2}{\epsilon_i - \epsilon_a + \epsilon_j - \epsilon_b}.\tag{[Szabo:1996], pp. 352, Eqn. 6.71}
# \end{equation}
#
# So far, our discussion has used spin-orbitals instead of the more familiar spatial orbitals.  Indeed, significant speedups are achieved when using spatial orbitals.  Integrating out the spin variable $\omega$ from $E_0^{(2)}$ yields two expressions; one each for the interaction of particles with the same spin (SS) and opposite spin (OS):
#
# \begin{align}
# E_{\rm 0,\,SS}^{(2)} = \sum_{ij}\sum_{ab}\frac{(ia\mid jb)[(ia\mid jb) - (ib\mid ja)]}{\epsilon_i - \epsilon_a + \epsilon_j - \epsilon_b}, \quad
# E_{\rm 0,\,OS}^{(2)} = \sum_{ij}\sum_{ab}\frac{(ia\mid jb)(ia\mid jb)}{\epsilon_i - \epsilon_a + \epsilon_j - \epsilon_b},
# \end{align}
#
# where these spin-free expressions make use of integrals in chemists' notation over spatial orbitals. (Rearranged from [Szabo:1996], pp. 352, Eqn. 6.74)  Note that an exchange integral arises between particles of the same spin; this is because the motions of particles with identical spins are correlated due to the requirement that $\left|\Psi\right|^2$ remain invariant to the exchange of the spatial and spin coordinates of any pair of electrons.  Finally, the total MP2 correction energy $E_0^{(2)} = E_{\rm 0,\,SS}^{(2)} + E_{\rm 0,\,OS}^{(2)}$.
#
# ### Implementation of Conventional MP2
#
# Let's begin by importing Psi4, NumPy and TensorOperations, and setting memory and output file options:

# +
# ==> Import statements & Global Options <==
using PyCall: pyimport
psi4 = pyimport("psi4")
np   = pyimport("numpy") # used only to cast to Psi4 arrays
using TensorOperations: @tensor

psi4.set_memory(Int(2e9))
numpy_memory = 2
psi4.core.set_output_file("output.dat", false)
# -

# Next, we can define our molecule and Psi4 options.  Notice that we are using `scf_type pk` to indicate that we wish to use conventional, full 4-index ERIs, and that we have specified `mp2_type conv` so that the MP2 algorithm we check against also uses the conventional ERIs.

# +
# ==> Molecule & Psi4 Options Definitions <==
mol = psi4.geometry("""
O
H 1 1.1
H 1 1.1 2 104
symmetry c1
""")

psi4.set_options(Dict("basis"         => "6-31g",
                      "scf_type"      => "pk",
                      "mp2_type"      => "conv",
                      "e_convergence" => 1e-8,
                      "d_convergence" => 1e-8))
# -

# Since MP2 is a perturbation on the zeroth-order Hartree-Fock description of a molecular system, all of the relevant information (Fock matrix, orbitals, orbital energies) about the system can be computed using any Hartree-Fock program.  We could use the RHF program that we wrote in tutorial 3a, but we could just as easily use Psi4 to do our dirty work.  In the cell below, use Psi4 to compute the RHF energy and wavefunction, and store them using the `return_wfn=True` keyword argument to `psi4.energy()`:

# Get the SCF wavefunction & energies
scf_e, scf_wfn = psi4.energy("scf", return_wfn=true)

# In the expression for $E_0^{(2)}$, the two summations are over occupied and virtual indices, respectively.  Therefore, we'll need to get the number of occupied orbitals and the total number of orbitals.  Additionally, we must obtain the MO energy eigenvalues; again since the sums are over occupied and virtual orbitals, it is good to separate the occupied orbital energies from the virtual orbital energies.  From the SCF wavefunction you generated above, get the number of doubly occupied orbitals, number of molecular orbitals, and MO energies:

# +
# ==> Get orbital information & energy eigenvalues <==
# Number of Occupied orbitals & MOs
ndocc = scf_wfn.nalpha()
nmo = scf_wfn.nmo()

# Get orbital energies, cast into NumPy array, and separate occupied & virtual
eps = np.asarray(scf_wfn.epsilon_a())
e_ij = eps[1:ndocc]
e_ab = eps[ndocc+1:end];
# -

# Unlike the orbital information, Psi4 does not return the ERIs when it does a computation.  Fortunately, however, we can just build them again using the `psi4.core.MintsHelper()` class.  Recall that these integrals will be generated in the AO basis; before using them in the $E_0^{(2)}$ expression, we must transform them into the MO basis.  To do this, we first need to obtain the orbital coefficient matrix, **C**.  In the cell below, generate the ERIs for our molecule, get **C** from the SCF wavefunction, and obtain occupied- and virtual-orbital slices of **C** for future use.

# +
# ==> ERIs <==
# Create instance of MintsHelper class
mints = psi4.core.MintsHelper(scf_wfn.basisset())

# Memory check for ERI tensor
I_size = nmo^4 * 8.e-9
println("\nSize of the ERI tensor will be $I_size GB.")
memory_footprint = I_size * 1.5
if I_size > numpy_memory
    psi4.core.clean()
    throw(OutOfMemoryError("Estimated memory utilization ($memory_footprint GB) exceeds " *
                           "allotted memory limit of $numpy_memory GB."))
end

# Build ERI Tensor
I = np.asarray(mints.ao_eri())

# Get MO coefficients from SCF wavefunction
C = np.asarray(scf_wfn.Ca())
Cocc = C[:, 1:ndocc]
Cvirt = C[:, ndocc+1:end];
# -

# In order to transform the four-index integrals from the AO to the MO basis, we must perform the following contraction:
#
# $$(i\,a\mid j\,b) = C_{\mu i}C_{\nu a}(\mu\,\nu\mid|\,\lambda\,\sigma)C_{\lambda j}C_{\sigma b}$$
#
# Again, here we are using $i,\,j$ as occupied orbital indices and $a,\, b$ as virtual orbital indices.  We could carry out the above contraction all in one step using either `@tensor` or explicit loops:
#
# ~~~julia
# # Naive Algorithm for ERI Transformation
# @tensor I_mo[i,a,j,b] := Cocc[p,i] * Cvirt[q,a] * I[p,q,r,s] * Cocc[r,j] * Cvirt[s,b]
# ~~~
#
# Notice that the transformation from AO index to occupied (virtual) MO index requires only the occupied (virtual) block of the **C** matrix; this allows for computational savings in large basis sets, where the virtual space can be very large.  This algorithm, while efficient with `@tensor`, has horrendous scaling if the search of an optimal contraction fails.  We will enforce a better contraction. Examining the contraction more closely, we see that there are 8 unique indices, and thus the step above scales as ${\cal O}(N^8)$.  With this algorithm, a twofold increase of the number of MO's would result in $2^8 = 256\times$ expense to perform.  We can, however, refactor the above contraction such that
#
# $$(i\,a\mid j\,b) = \left[C_{\mu i}\left[C_{\nu a}\left[C_{\lambda j}\left[C_{\sigma b}(\mu\,\nu\mid|\,\lambda\,\sigma)\right]\right]\right]\right],$$
#
# where we have now written the transfomation as four ${\cal O}(N^5)$ steps instead of one ${\cal O}(N^8)$ step. This is a savings of $\frac{4}{n^3}$, and is responsible for the feasibility of the MP2 method for application to any but very small systems and/or basis sets.  We may carry out the above ${\cal O}(N^5)$ algorithm by carrying out one index transformation at a time, and storing the result in a temporary array.  In the cell below, transform the ERIs from the AO to MO basis, using our smarter algorithm:

# ==> Transform I -> I_mo @ O(N⁵) <==
I_mo = @tensor begin
   I_mo[i,q,r,s] := Cocc[p,i]     * I[p,q,r,s]
   I_mo[i,a,r,s] := Cvirt[q,a]    * I_mo[i,q,r,s]
   I_mo[i,a,j,s] :=                 I_mo[i,a,r,s] * Cocc[r,j]
   I_mo[i,a,j,b] :=                 I_mo[i,a,j,s] * Cvirt[s,b]
end
nothing

# We note here that we can use infrastructure in Psi4 to carry out the above integral transformation; this entails obtaining the occupied and virtual blocks of **C** Psi4-side, and then using the built-in `MintsHelper` function `MintsHelper.mo_eri()` to transform the integrals.  Just to check your work above, execute the next cell to see this tech in action:

# ==> Compare our Imo to MintsHelper <==
Co = scf_wfn.Ca_subset("AO","OCC")
Cv = scf_wfn.Ca_subset("AO","VIR")
MO = np.asarray(mints.mo_eri(Co, Cv, Co, Cv))
println("Do our transformed ERIs match Psi4's? ", np.allclose(I_mo, np.asarray(MO)))

# Now we have all the pieces needed to compute $E_0^{(2)}$.  This could be done by writing explicit loops over occupied and virtual indices Julia side, e.g.,

# +
# Compute SS & OS MP2 Correlation
mp2_corr = let mp2_ss_corr = 0.0, mp2_os_corr = 0.0
   nvirt = nmo - ndocc
   for i in 1:ndocc, a in 1:nvirt, j in 1:ndocc, b in 1:nvirt
       numerator = I_mo[i,a,j,b] * (I_mo[i, a, j, b] - I_mo[i, b, j, a])
       mp2_ss_corr += numerator / (e_ij[i] + e_ij[j] - e_ab[a] - e_ab[b])
       mp2_os_corr += I_mo[i,a,j,b]^2 / (e_ij[i] + e_ij[j] - e_ab[a] - e_ab[b])
   end
   mp2_ss_corr + mp2_os_corr
end

# Total MP2 Energy
MP2_E = scf_e + mp2_corr

# ==> Compare to Psi4 <==
psi4.compare_values(psi4.energy("mp2"), MP2_E, 6, "MP2 Energy")
# -

# In this method it is very clear what is going on and is easy to program. Julia has the distinct advantage loops are as fast as the same block written in a compiled language like C, C++, or Fortran.
#
# However, we will provide an alternative formulation using tensor contractions. It should be clear how to contract the four-index integrals $(i\,a\mid j\,b)$ and $(i\,a\mid j\,b)$ with one another, but what about the energy eigenvalues $\epsilon$?  We can use a Julia trick called *broadcasting* to construct a four-index array of all possible energy denominators, which can then be contracted with the full I_mo arrays.  To do this, we'll use the function `reshape()`:
# ~~~julia
# # Prepare 4d energy denominator array
# e_denom  = reshape(e_ij,  1, 1, 1, :)      # Diagonal of 4d array are occupied orbital energies
# e_denom -= reshape(e_ab', 1, 1, :)         # all combinations of (e_ij - e_ab)
# e_denom += e_ij                            # all combinations of [(e_ij - e_ab) + e_ij]
# e_denom -= e_ab'                           # All combinations of full denominator
# e_denom  = premutedims(e_denom, (1,2,4,3)) # permute 3rd and 4th dims to have (nocc,nvirt,nocc,nvirt) shape
# e_denom  = inv.(e_denom)                   # Take reciprocal for contracting with numerator
# ~~~
# In the cell below, compute the energy denominator using `reshape()` and contract this array with the four-index ERIs to compute the same-spin and opposite-spin MP2 correction using `sum()`. Then, add these quantities to the SCF energy computed above to obtain the total MP2 energy.
#
# Hint: For the opposite-spin correlation, use `permutedims()` to obtain the correct ordering of the indices in the exchange integral.

# +
#using Einsum: @einsum
# ==> Compute MP2 Correlation & MP2 Energy <==
# Compute energy denominator array
e_denom = reshape(e_ij,1,1,1,:) .- reshape(e_ab',1,1,:) .+ (e_ij .- e_ab')
e_denom = permutedims(e_denom, (1,2,4,3)) # 3 ↔ 4
e_denom = inv.(e_denom)

# check
#using Test
#nvirt = nmo - ndocc
#for i in 1:ndocc, a in 1:nvirt, j in 1:ndocc, b in 1:nvirt
#    @test e_denom[i,a,j,b] ≈ 1 / (e_ij[i] + e_ij[j] - e_ab[a] - e_ab[b])
#end

# Compute SS & OS MP2 Correlation with sum()
bctd_mp2_os_corr = sum(I_mo .* I_mo .* e_denom)
I_mo_swap = permutedims(I_mo,(3,2,1,4)) # 1 ↔ 3
bctd_mp2_ss_corr = sum(I_mo .* (I_mo .- I_mo_swap) .* e_denom)

# Compare broadcasted and loop MP2
@assert bctd_mp2_os_corr + bctd_mp2_ss_corr ≈ mp2_corr

# Total MP2 Energy
MP2_E = scf_e + bctd_mp2_os_corr + bctd_mp2_ss_corr
# -

# ==> Compare to Psi4 <==
psi4.compare_values(psi4.energy("mp2"), MP2_E, 6, "MP2 Energy")

# ## References
#
# 1. Original paper: "Note on an Approximation Treatment for Many-Electron Systems"
# 	> [[Moller:1934:618](https://journals.aps.org/pr/abstract/10.1103/PhysRev.46.618)] C. Møller and M. S. Plesset, *Phys. Rev.* **46**, 618 (1934)
# 2. The Laplace-transformation in MP theory: "Minimax approximation for the decomposition of energy denominators in Laplace-transformed Møller–Plesset perturbation theories"
#     > [[Takasuka:2008:044112](http://aip.scitation.org/doi/10.1063/1.2958921)] A. Takatsuka, T. Siichiro, and W. Hackbusch, *J. Phys. Chem.*, **129**, 044112 (2008)
# 3. Equations taken from:
# 	> [[Szabo:1996](https://books.google.com/books?id=KQ3DAgAAQBAJ&printsec=frontcover&dq=szabo+%26+ostlund&hl=en&sa=X&ved=0ahUKEwiYhv6A8YjUAhXLSCYKHdH5AJ4Q6AEIJjAA#v=onepage&q=szabo%20%26%20ostlund&f=false)] A. Szabo and N. S. Ostlund, *Modern Quantum Chemistry: Introduction to Advanced Electronic Structure Theory*. Courier Corporation, 1996.
# 4. Algorithms taken from:
# 	> [Crawford:prog] T. D. Crawford, "The Second-Order Møller–Plesset Perturbation Theory (MP2) Energy."  Accessed via the web at http://github.com/CrawfordGroup/ProgrammingProjects.


text:
ENERGY_DATA = Dict{String, Int}()
const MAXENERGY = 20

function energy(id::String)
    global ENERGY_DATA, MAXENERGY
    if id in ENERGY_DATA
        ENERGY_DATA[id]
    else
        ENERGY_DATA[id] = MAXENERGY
        energy(id)
    end
end
energy(ply::Player) = energy(ply.id)

function init_cache()
    return (PlayerCache(), WorldCache())
end

"""
Returns the DB at the default location.
"""
getDB() = SQLite.DB("D:\\Code\\.julia\\dev\\Elvur\\data\\wrld.db")

include("player.jl")
include("world.jl")


text:
module Grids

using Test
using Gridap
using Gridap.Helpers
using Gridap.CellValuesGallery

export Grid
export points
export cells
export celltypes
export cellorders
export test_grid
export celldim
export pointdim
import Gridap: ncells
export npoints
export CellPolytopes
import Gridap: Triangulation
import Gridap: CellPoints
import Gridap: CellRefFEs
import Gridap: CellBasis

# Interfaces

"""
Abstract type representing a FE mesh a.k.a. grid (i.e., there is a 1 to 1
correspondence between a grid and a conforming Lagrangian FE space with no
Dirichlet boundary conditions).
D is the dimension of the coordinates and Z is the dimension of the cells.
"""
abstract type Grid{D,Z} end

function points(::Grid{D})::IndexCellValue{<:Point{D}} where D
  @abstractmethod
end

cells(::Grid)::IndexCellArray{Int,1} = @abstractmethod

function celltypes(::Grid{D,Z})::CellValue{NTuple{Z,Int}} where {D,Z}
  @abstractmethod
end

cellorders(::Grid)::CellValue{Int} = @abstractmethod

celldim(::Grid{D,Z}) where {D,Z} = Z

pointdim(::Grid{D,Z}) where {D,Z} = D

ncells(g::Grid) = length(celltypes(g))

npoints(g::Grid) = length(points(g))

CellPolytopes(g::Grid) = _cell_polytopes(celltypes(g))

Triangulation(grid::Grid) = TriangulationFromGrid(grid)

# Testers

function test_grid(grid::Grid{D,Z},np,nc) where {D,Z}
  @test isa(points(grid),IndexCellValue{<:Point{D}})
  @test isa(cells(grid),IndexCellVector{<:Integer})
  @test isa(celltypes(grid),CellValue{NTuple{Z,Int}})
  @test isa(cellorders(grid),IndexCellValue{Int})
  @test np == length(points(grid))
  @test nc == length(cells(grid))
  @test nc == length(celltypes(grid))
  @test nc == length(cellorders(grid))
  trian = Triangulation(grid)
  test_triangulation(trian)
end

# Pretty printing

import Base: show

function show(io::IO,self::Grid{D,Z}) where {D,Z}
  print(io,"$(nameof(typeof(self))) object")
end

function show(io::IO,::MIME"text/plain",grid::Grid)
  show(io,grid)
  print(io,":")
  print(io,"\n pointdim: $(pointdim(grid))")
  print(io,"\n celldim: $(celldim(grid))")
  print(io,"\n npoints: $(npoints(grid))")
  print(io,"\n ncells: $(ncells(grid))")
end

# Concrete implementation

struct GridFromData{D,Z} <: Grid{D,Z}
  _points::IndexCellValue{<:Point{D}}
  _cells::IndexCellVector
  _celltypes::IndexCellValue{NTuple{Z,Int}}
  _cellorders::IndexCellValue{Int}
end

points(g::GridFromData) = g._points

cells(g::GridFromData) = g._cells

celltypes(g::GridFromData) = g._celltypes

cellorders(g::GridFromData) = g._cellorders

# Helpers

struct TriangulationFromGrid{D,Z,G<:Grid{D,Z}} <: Triangulation{Z,D}
  grid::G
end

function CellPoints(self::TriangulationFromGrid)
  CellVectorFromLocalToGlobal(cells(self.grid),points(self.grid))
end

function CellRefFEs(trian::TriangulationFromGrid)
  grid = trian.grid
  _build_cell_ref_fes(celltypes(grid),cellorders(grid))
end

function CellBasis(trian::TriangulationFromGrid)
  reffes = CellRefFEs(trian)
  _setup_cell_basis(reffes)
end

function _build_cell_ref_fes(codes,orders)
  @notimplemented
end

function _build_cell_ref_fes(
  codes::ConstantCellValue, orders::ConstantCellValue)
  code = codes.value
  D = length(code)
  order = orders.value
  polytope = Polytope(code)
  _orders = fill(order,D)
  reffe = LagrangianRefFE(Float64,polytope,_orders)
  ConstantCellValue(reffe,length(codes))
end

function _setup_cell_basis(reffes)
  @notimplemented
end

function _setup_cell_basis(reffes::ConstantCellValue)
  reffe = reffes.value
  basis = shfbasis(reffe)
  ConstantCellMap(basis,reffes.length)
end

function _cell_polytopes(ct)
  @notimplemented
end

function _cell_polytopes(ct::ConstantCellValue)
  extr = ct.value
  l = ct.length
  p = Polytope(extr)
  ConstantCellValue(p,l)
end

end # module


text:
@testset "Multilinear Expressions" begin

    @info "Multilinear Expressions"
    MLE = MultilinearExpression # Alias
    @show c = MLE(7.4)
    @show isa(c,MLE)
    @show e1 = MLE(4.0,1)
    @show isa(e1,MLE)
    @show e2 = MLE(3.0,1,3)
    @show e3 = c + e1 + e2
    @show isa(e3,MLE)
    @show e3[Monomial((1,3))]
    @show e3[Monomial((1,2))]
    @show e3[Monomial((1,2))] = 1.5
    @show length(e3)
    for (m,c) in e3
        println(m => c)
    end
    @show e4 = MLE(1.0,1) + MLE(1.5,1,2)
    @show isa(e4,MLE)
    @show e5 = e3 + e4
    @show e6 = e5 + 2.6
    @show e7 = -2.4 + e5
    @show e8 = e7 + zero(e7)
    @show e9 = e8 + 0.2 * one(e8)
    @show Monomial((1,)) * Monomial((3,))
    try
        @show e1 * e2
    catch
        println("Multiplication of expressions with common variables produces error")
    end
    @show e1 * MLE(0.5,2)
    @show MLE(2.0,4) * e7
    @show a = MLE(2.0,1,3) + MLE(2.0,3)
    @show b = MLE(3.0,2) + MLE(1.0,2,4) + MLE(1.0,4)
    @show c = a * b
    @show a == b
    @show a == MLE(2.0,1,3) + MLE(2.0,3)
    @show c == a * b
    @show isequal(c,a * b)
    @show MLE(0) == MLE(0)
    @show 1+MLE(-1.0,1) == 1+MLE(-1.0,1)
    @show hash(Monomial((1,3)))
    @show hash(a)
    @show hash(MLE(2.0,1,3) + MLE(2.0,3))
    @show hash(c)
    @show hash(a * b)
    println()

end # testset


text:
"""ラグランジュ法による入力計算（テスト）
"""

"""
ロボットの動力学

運動学とDHパラメータの取り方が異なることに注意
"""
module Dynamics

using LinearAlgebra

export calc_torque
export calc_real_ddq

const n = 7

const Ixx = (
    0.0470910226,
    0.027885975,
    0.0266173355,
    0.0131822787,
    0.0166774282,
    0.0070053791,
    0.0008162135,
)

const Iyy = (
    0.035959884,
    0.020787492,
    0.012480083,
    0.009268520,
    0.003746311,
    0.005527552,
    0.0008735012,
)

const Izz = (
    0.0376697645,
    0.0117520941,
    0.0284435520,
    0.0071158268,
    0.0167545726,
    0.0038760715,
    0.0005494148,
)

const Ixy = (
    -0.0061487003,
    -0.0001882199,
    -0.0039218988,
    -0.0001966341,
    -0.0001865762,
    0.0001534806,
    0.000128440,
)

const Iyz = (
    -0.0007808689,
    0.0020767576,
    -0.001083893,
    0.000745949,
    0.0006473235,
    -0.0002111503,
    0.0001057726,
)

const Ixz = (
    0.0001278755,
    -0.00030096397,
    0.0002927063,
    0.0003603617,
    0.0001840370,
    -0.0004438478,
    0.00018969891,
)

const x_bar = (
    -0.05117,
    0.00269,
    -0.07176,
    0.00159,
    -0.01168,
    0.00697,
    0.005137,
)

const y_bar = (
    0.07908,
    -0.00529,
    0.08149,
    -0.01117,
    0.13111,
    0.006,
    0.0009572,
)

const z_bar = (
    0.00086,
    0.06845,
    0.00132,
    0.02618,
    0.0046,
    0.06048,
    -0.06682,
)

const m = (
    5.70044,
    3.22698,
    4.31272,
    2.07206,
    2.24665,
    1.60979,
    0.54218,
)

const d = (
    0.2703,
    0.0,
    0.3644,
    0.0,
    0.3743,
    0.0,
    0.2295,
)

const a = (
    0.069,
    0.0,
    0.069,
    0.0,
    0.01,
    0.0,
    0.0,
)

const alpha = (
    -pi/2,
    pi/2,
    -pi/2,
    pi/2,
    -pi/2,
    pi/2,
    0.0,
)

const Q = [
    0.0 -1.0 0.0 0.0
    1.0 0.0 0.0 0.0
    0.0 0.0 0.0 0.0
    0.0 0.0 0.0 0.0
]  # 偏微分演算行列

const g = [0.0, 0.0, -9.81, 0.0]'  # 重力加速度ベクトル（横ベクトル）

function r_bar(i::Int64)
    [
        x_bar[i]
        y_bar[i]
        z_bar[i]
        1.0
    ]
end

"""
(i-1)T(i)

thetaはq[i]です
"""
function T(i::Int64, theta::Float64)
    return [
        cos(theta) -cos(alpha[i])*sin(theta) sin(alpha[i])*sin(theta) a[i]*cos(theta)
        sin(theta) cos(alpha[i])*cos(theta) -sin(alpha[i])*cos(theta) a[i]*sin(theta)
        0.0 sin(alpha[i]) cos(alpha[i]) d[i]
        0.0 0.0 0.0 1.0
    ]
end

function T(i::Int64, j::Int64, q::Vector{TU}) where TU
    z = T(i, q[i])
    _z = similar(z)
    for k in i+1:j
        ### 順番がわからない

        #z =  z * T(k, q[k])
        z = T(k, q[k]) * z  # 間違いなのに正しい結果
        #mul!(_z, T(k, q[k]), z)
        #copy!(z, _z)
    end
    z
end

"""慣性モーメント"""
function J(i::Int64)
    [
        (-Ixx[i]+Iyy[i]+Izz[i])/2 Ixy[i]                   Ixz[i]                   m[i]*x_bar[i]
        Ixy[i]                    (Ixx[i]-Iyy[i]+Izz[i])/2 Iyz[i]                   m[i]*y_bar[i]
        Ixz[i]                    Iyz[i]                   (Ixx[i]+Iyy[i]-Izz[i])/2 m[i]*z_bar[i]
        m[i]*x_bar[i]             m[i]*y_bar[i]            m[i]*z_bar[i]            m[i]
    ]
end

function U(i::Int64, j::Int64, q::Vector{TU}) where TU
    if j <= i
        return T(1, j-1, q) * Q * T(j, i, q)
    else
        return zeros(TU, 4, 4)
    end
end

function U(i::Int64, j::Int64, k::Int64, q::Vector{TU}) where TU
    if i >= k >= j
        return T(1, j-1, q) * Q * T(j, k-1, q) * Q * T(k, i, q)
    elseif i >= j >= k
        return T(1, k-1, q) * Q * T(k, j-1, q) * Q * T(j, i, q)
    else
        return zeros(TU, 4, 4)
    end
end

### 慣性，コリオリ，重力 ###
"""慣性行列の要素"""
function M(i::Int64, k::Int64, q::Vector{TU}) where TU
    j_start = max(i, k)
    z = 0.0
    for j in j_start:n
        #println(j)
        #println(U(j, k, q))
        z += tr(U(j, k, q) * J(j) * U(j, i, q)')
    end
    z
end

function h(i::Int64, k::Int64, m::Int64, q::Vector{TU}) where TU
    j_start = max(i, k, m)
    z = 0.0
    for j in j_start:n
        z += tr(U(j, k, m, q) * J(i) * U(j, i, q)')
    end
    z
end

"""コリオリ行列の要素"""
function C(i::Int64, q::Vector{TU}, dq::Vector{TU}) where TU
    z = 0.0
    for k in 1:n
        for m = 1:n
            z += h(i, k, m, q) * dq[k] * dq[m]
        end
    end
    z
end

"""重力行列の要素"""
function G(i::Int64, q::Vector{TU}) where TU
    z = 0.0
    for j in i:n
        z += -m[j] * g * U(j, i, q) * r_bar(j)
    end
    z
end

### 目的の行列作成 ###

"""慣性行列"""
function M(q::Vector{TU}) where TU
    z = zeros(TU, 7, 7)
    for i in 1:7
        for j in 1:7
            z[i, j] = M(i, j, q)
        end
    end
    z
end

"""コリオリ行列"""
function C(q::Vector{TU}, dq::Vector{TU}) where TU
    z = zeros(TU, 7, 1)
    for i in 1:7
        z[i] = C(i, q, dq)
    end
    z
end

"""重力行列"""
function G(q::Vector{TU}) where TU
    z = zeros(TU, 7, 1)
    for i in 1:7
        z[i] = G(i, q)
    end
    z
end

"""トルクを計算

計算トルク法です
q : 関節角度ベクトル
dq : 関節角速度ベクトル
desired_ddq : （所望の）関節角速度ベクトル
"""
function calc_torque(
    q::Vector{TU}, dq::Vector{TU}, desired_ddq::Vector{TU}
    ) where TU
    M(q)*desired_ddq .+ C(q, dq) .+ G(q) |> vec
end

"""現実世界での加速度

u : 入力トルクベクトル R(7)
q : （現在の）関節角度ベクトル
dq : （現在の）関節角速度ベクトル
F : 外乱ベクトル R(7)
Fc : エンドエフェクタに加わる外力ベクトル
Jend : エンドエフェクタのヤコビ行列
"""
function calc_real_ddq(;
    u::Vector{TU}, q::Vector{TU}, dq::Vector{TU},
    F::Vector{TU}, Fc::Vector{TU},
    Jend::Matrix{TU}
    ) where TU

    # println(size(transpose(Jend)))
    # println(transpose(Jend) * Fc)
    # println(F)

    _Fc = transpose(Jend) * Fc
    @. F += _Fc

    # println(F)

    inv(M(q)) * (u .+ F .- (C(q, dq) .+ G(q))) |> vec
end

#export calc_real_ddq

"""テスト用"""
function _test()
    q = [
        0.0
        -31
        0.0
        43.0
        0.0
        72.0
        0.0
    ] * pi / 180
    dq = q
    ddq = q

    #i = 2
    #j = 6
    #println(T(i, j, q))

    i = 3
    j = 5
    println(det(M(q)))
    D = eigvals(M(q))
    println(D)
    #println(P)

    #println(M(q))
    #u = calc_torque(q, dq, ddq)
    #println(u)
    #F = zeros(Float64, 7)
    #r_ddq = calc_real_ddq(u, F, q, dq)
    #println(r_ddq)
    #println(typeof(r_ddq))
end

end

#@time for i in 1:100; _test(); end
#@time for i in 1:10; Dynamics._test(); end

# using .Dynamics

# Dynamics._test()


text:
module DiffModels

import Base: rand
import Distributions: pdf, sampler, twoπ, sqrt2π
import Distributions: Sampleable, ValueSupport, Univariate, Multivariate

using Compat, SpecialFunctions

export
    # generic types
    AbstractDrift,
    AbstractBound,
    AbstractBounds,

    # types
    ConstDrift,
    VarDrift,
    ConstBound,
    VarBound,
    LinearBound,
    SymBounds,
    ConstBounds,
    ConstSymBounds,
    LinearSymBounds,
    VarSymBounds,
    AsymBounds,
    ConstAsymBounds,
    VarAsymBounds,
    DMBoundOutcome,
    DMBoundsOutcome,

    # methods
    rand,
    sampler,
    pdf,
    pdfu,
    pdfl,
    pdful

include("common.jl")
include("drift.jl")
include("bound.jl")
include("diffusion.jl")
include("fpt.jl")
include("sim.jl")

end # module


text:
#GracePlot demonstration 2: Multi-graph plot stress test
#-------------------------------------------------------------------------------

using GracePlot

#==Input data
===============================================================================#
x=collect(-10:0.1:10)
y2 = x.^2
y3 = x.^3

#=="Defaults"
===============================================================================#
pdefaults = defaults(linewidth=2.5, linestyle=:solid)
thinline = line(width=0.5)
thickline = line(style=:ldash, width=8, color=1)
thickframeline = line(width=2.5)
loglin = axes(xscale = :log, yscale = :lin)
stdleg = legend(loc=(.9, 1/3-1/20), loctype=:view, charsize=.75)

#==Generate plot
===============================================================================#
plot = GracePlot.new()
@show get(plot, :wview), get(plot, :hview) #Not useful yet
set(plot, pdefaults)
#Try a multi-graph plot:
arrange(plot, (3, 2), offset=0.08, hgap=0.15, vgap=0.3)
g = graph(plot, (2, 0)) #Get a reference to graph (1,2)
	txt = text("Parabola (\\f{Times-Italic}y=x\\f{}\\S2\\N)", size=2)
	set(g, stdleg, xlabel = text("x-axis", color=2), ylabel = "y-axis")
	set(g, unsupported = "no dice")
	set(g, subtitle = txt)
	set(g, frameline = thickframeline)
	#Add datasets:
	ds = add(g, x, y2, id="y^2")
		set(ds, glyph(shape=:diamond, color=5), glyph(skip=10))
		set(ds, line(color=3))
	ds = add(g, x, 2.*y2, id="2y^2")
		set(ds, glyph(shape=:char, char=Int('!'), size=2, skip=10))
	autofit(g)
g = graph(plot, (0, 1))
	set(g, subtitle = "Cubic Function (\\f{Times-Italic}y=x\\f{}\\S3\\N)")
	#Add datasets:
	ds = add(g, x, y3, thickline)
		set(ds, line(style=3, width=8, color=1)) #Overwrite defaults
	autofit(g)
	#autofit(g, x=true) #Not supported by Grace v5.1.23?
g = graph(plot, (1, 1)) #Play around with another graph
	#Test message logging functionality:
	plot.log = true
	set(g, axes(xmin = 0.1, xmax = 1000, ymin = 1000, ymax = 5000))
	set(g, loglin, axes(inverty = :on))
	plot.log = false

#Finalize:
set(plot, focus=g)
redraw(plot)

#Other possible operations... but don't test here:
#sleep(1); close(plot)

#Last line


text:
using Bokeh
# by default autoopen is false (since v0.0.2) so to cause plots to show
# when plot is called we switch it on
# this can also be achieved by passing autoopen=true to plot
autoopen(true)

# for consistency we set the name of the html file to generate in this example
# if you dont set this the default name "bokeh_plot.html" is used
plotfile("simple.html")

# more or less the simplest plotting example
x = linspace(0, 2pi)
y = sin.(x)
# autoopen is set to true so `plot` will try to open a browser to show the plot
plot(x, y)


text:
function export_gms(path)
    cd(path)
    model = ReSHOP.Optimizer()
    ReSHOP.setexport(true)

    MOI.empty!(model)

    x = MOI.add_variable(model)
    y = MOI.add_variable(model)
    t = MOI.add_variable(model)

    c1f = MOI.ScalarAffineFunction(MOI.ScalarAffineTerm.([1.0, 1.0], [x,y]), 0.0)
    c1 = MOI.add_constraint(model, c1f, MOI.GreaterThan(1.0))

    c2f = MOI.ScalarQuadraticFunction(
        MOI.ScalarAffineTerm{Float64}[],
        MOI.ScalarQuadraticTerm.([2.0, 2.0, -2.0], [x, y, t], [x, y, t]),
        0.0
    )
    c2 = MOI.add_constraint(model, c2f, MOI.LessThan(0.0))

    bound = MOI.add_constraint(model, MOI.SingleVariable(t), MOI.GreaterThan(0.0))

    MOI.set(model, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{Float64}}(), MOI.ScalarAffineFunction([MOI.ScalarAffineTerm(1.0, t)], 0.0))
    MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE)

    MOI.optimize!(model)
end