text:
module TableReader

export readdlm, readtsv, readcsv

using DataFrames:
    DataFrame
using TranscodingStreams:
    TranscodingStreams,
    TranscodingStream,
    NoopStream,
    Memory,
    Buffer,
    State,
    Noop,
    fillbuffer,
    buffermem
using CodecZlib:
    GzipDecompressorStream
using CodecZstd:
    ZstdDecompressorStream
using CodecXz:
    XzDecompressorStream

const DEFAULT_CHUNK_SIZE =  1 * 2^20  #  1 MiB
const MINIMUM_CHUNK_SIZE = 16 * 2^10  # 16 KiB

"""
    readdlm(filename or IO object;
            delim,
            quot = '"',
            trim = true,
            header = nothing,
            chunksize = $(DEFAULT_CHUNK_SIZE))

Read a character delimited text file.

`delim` specifies the field delimiter in a line. This must be tab, space, or an
ASCII punctuation character.

`quot` specifies the quotation to enclose a field. This cannot be the same
character as `delim`.

`trim` specifies whether the parser trims space (0x20) characters around a field.
If `trim` is true, `delim` and `quot` cannot be a space character.

`header` specifies the column names. If `header` is `nothing` (default), the
column names are read from the first line of the text file. Any iterable object
is allowed.

If unnamed columns are found in the header, they are renamed to `UNNAMED_{j}`
for ease of access, where `{j}` is replaced by the column number.

A text file will be read chunk by chunk to save memory. The chunk size is
specified by the `chunksize` parameter, which is set to 1 MiB by default.
The data type of each column is guessed from the values in the first chunk.
"""
function readdlm end

"""
    readtsv(filename or IO object; delim = '\\t', <keyword arguments>)

Read a TSV text file.

This function is the same as [`readdlm`](@ref) but with `delim = '\\t'`.
See `readdlm` for details.
"""
function readtsv end

"""
    readcsv(filename or IO object; delim = ',', <keyword arguments>)

Read a CSV text file.

This function is the same as [`readdlm`](@ref) but with `delim = ','`.
"""
function readcsv end

for (fname, delim) in [(:readdlm, nothing), (:readtsv, '\t'), (:readcsv, ',')]
    # prepare keyword arguments
    kwargs = Expr[]
    if delim === nothing
        push!(kwargs, :(delim::Char))
    else
        push!(kwargs, Expr(:kw, :(delim::Char), delim))
    end
    push!(kwargs, Expr(:kw, :(quot::Char), '"'))  # quot::Char = '"'
    push!(kwargs, Expr(:kw, :(trim::Bool), true))  # trim::Bool = true
    push!(kwargs, Expr(:kw, :(header), nothing))  # header = nothing
    push!(kwargs, Expr(:kw, :(chunksize::Integer), DEFAULT_CHUNK_SIZE))  # chunksize::Integer = DEFAULT_CHUNK_SIZE

    # generate methods
    @eval begin
        function $(fname)(filename::AbstractString; $(kwargs...))
            params = check_parser_parameters(delim, quot, trim, header, chunksize)
            if occursin(r"^\w+://", filename)  # URL-like filename
                if Sys.which("curl") === nothing
                    throw(ArgumentError("the curl command is not available"))
                end
                # read a remote file using curl
                return open(proc -> readdlm_internal(wrapstream(proc, params), params), `curl --silent $(filename)`)
            end
            # read a local file
            return open(file -> readdlm_internal(wrapstream(file, params), params), filename)
        end

        function $(fname)(cmd::Base.AbstractCmd; $(kwargs...))
            params = check_parser_parameters(delim, quot, trim, header, chunksize)
            return open(proc -> readdlm_internal(wrapstream(proc, params), params), cmd)
        end

        function $(fname)(file::IO; $(kwargs...))
            params = check_parser_parameters(delim, quot, trim, header, chunksize)
            return readdlm_internal(wrapstream(file, params), params)
        end
    end
end

const SP = UInt8(' ')
const CR = UInt8('\r')
const LF = UInt8('\n')

# Printable characters
const CHARS_PRINT = ' ':'~'

# Whitelist of delimiters
const ALLOWED_DELIMITERS = tuple(['\t'; ' '; CHARS_PRINT[ispunct.(CHARS_PRINT)]]...)

# A set of parser parameters.
struct ParserParameters
    delim::UInt8
    quot::UInt8
    trim::Bool
    colnames::Union{Vector{Symbol},Nothing}
    chunksize::Int
end

function check_parser_parameters(delim::Char, quot::Char, trim::Bool, header::Any, chunksize::Integer)
    if delim ∉ ALLOWED_DELIMITERS
        throw(ArgumentError("delimiter $(repr(delim)) is not allowed"))
    elseif quot ∉ ALLOWED_DELIMITERS
        throw(ArgumentError("quotation mark $(repr(quot)) is not allowed"))
    elseif delim == quot
        throw(ArgumentError("delimiter and quotation mark cannot be the same character"))
    elseif delim == ' ' && trim
        throw(ArgumentError("delimiting with space and space trimming are exclusive"))
    elseif quot == ' ' && trim
        throw(ArgumentError("quoting with space and space trimming are exclusive"))
    elseif chunksize < 0
        throw(ArgumentError("chunks size cannot be negative"))
    end
    if header != nothing
        colnames = Symbol.(collect(header))
    else
        colnames = nothing
    end
    return ParserParameters(
        UInt8(delim),
        UInt8(quot),
        trim,
        colnames,
        chunksize,
    )
end

# Wrap a stream by TranscodingStream.
function wrapstream(stream::IO, params::ParserParameters)
    bufsize = max(params.chunksize, MINIMUM_CHUNK_SIZE)
    if !applicable(mark, stream) || !applicable(reset, stream)
        stream = NoopStream(stream, bufsize = bufsize)
    end
    format = checkformat(stream)
    if params.chunksize != 0
        # with chunking
        if format == :gzip
            return GzipDecompressorStream(stream, bufsize = bufsize)
        elseif format == :zstd
            return ZstdDecompressorStream(stream, bufsize = bufsize)
        elseif format == :xz
            return XzDecompressorStream(stream, bufsize = bufsize)
        elseif stream isa TranscodingStream
            return stream
        else
            return NoopStream(stream, bufsize = bufsize)
        end
    end
    # without chunking
    if format == :gzip
        data = read(GzipDecompressorStream(stream))
    elseif format == :zstd
        data = read(ZstdDecompressorStream(stream))
    elseif format == :xz
        data = read(XzDecompressorStream(stream))
    else
        data = read(stream)
    end
    buffer = Buffer(data)
    return TranscodingStream(Noop(), devnull, State(buffer, buffer))
end

# Check the file format of a stream.
function checkformat(stream::IO)
    mark(stream)
    magic = zeros(UInt8, 6)
    nb = readbytes!(stream, magic, 6)
    reset(stream)
    if nb != 6
        return :unknown
    elseif magic[1:6] == b"\xFD\x37\x7A\x58\x5A\x00"
        return :xz
    elseif magic[1:2] == b"\x1f\x8b"
        return :gzip
    elseif magic[1:4] == b"\x28\xb5\x2f\xfd"
        return :zstd
    else
        return :unknown
    end
end

function readdlm_internal(stream::TranscodingStream, params::ParserParameters)
    delim, quot, trim = params.delim, params.quot, params.trim
    chunking = params.chunksize != 0
    if params.colnames === nothing
        colnames = readheader(stream, delim, quot, trim)
    else
        colnames = params.colnames
    end
    if any(name -> name === Symbol(""), colnames)
        rename_unnamed_columns!(colnames)
    end
    ncols = length(colnames)
    if ncols == 0
        return DataFrame()
    end
    fillbuffer(stream, eager = true)
    buffer = stream.state.buffer1
    nrows_estimated = countlines(buffermem(buffer))
    if nrows_estimated == 0
        nrows_estimated = countlines(buffermem(buffer), byte = CR)
    end
    nrows_estimated += 1  # maybe insert a newline at the end
    n_chunk_rows = nrows_estimated
    tokens = Array{Token}(undef, (ncols, n_chunk_rows))
    #fill!(tokens, Token(0x00, 0, 0))
    columns = Vector[]
    line = 2
    while !eof(stream)
        mem = buffermem(buffer)
        lastnl = find_last_newline(mem)
        if lastnl == 0 && fillbuffer(stream, eager = true) == 0
            # reached EOF without newline marker, so insert an LF to cheat the parser
            TranscodingStreams.makemargin!(buffer, 1)
            TranscodingStreams.writebyte!(buffer, LF)
            mem = buffermem(buffer)
            lastnl = find_last_newline(mem)
        end
        # maybe, found a line that is too long?
        @assert lastnl > 0
        pos = 0
        chunk_begin = line
        if chunking
            while pos < lastnl && line - chunk_begin + 1 ≤ n_chunk_rows
                pos = scanline!(tokens, line - chunk_begin + 1, mem, pos, lastnl, line, delim, quot, trim)
                line += 1
            end
        else
            while pos < lastnl
                @assert line - chunk_begin + 1 ≤ n_chunk_rows
                pos = scanline!(tokens, line - chunk_begin + 1, mem, pos, lastnl, line, delim, quot, trim)
                line += 1
            end
        end
        n_new_records = line - chunk_begin
        bitmaps = aggregate_columns(tokens, n_new_records)
        if isempty(columns)
            # infer data types of columns
            resize!(columns, ncols)
            for i in 1:ncols
                parsable = bitmaps[i] & 0b0111
                hasmissing = (bitmaps[i] & 0b1000) != 0
                T = (parsable & INTEGER) != 0 ? Int :
                    (parsable & FLOAT) != 0 ? Float64 : String
                if hasmissing
                    T = Union{T,Missing}
                end
                col = Vector{T}(undef, n_new_records)
                fillcolumn!(col, n_new_records, mem, tokens, i, quot)
                columns[i] = col
            end
        else
            # check existing columns
            for i in 1:ncols
                parsable = bitmaps[i] & 0b0111
                hasmissing = (bitmaps[i] & 0b1000) != 0
                col = columns[i]
                T = eltype(col)
                if T <: Union{Int,Missing}
                    (parsable & INTEGER) == 0 && throw(ReadError("type guessing failed at column $(i)"))
                elseif T <: Union{Float64,Missing}
                    (parsable & FLOAT) == 0 && throw(ReadError("type guessing failed at column $(i)"))
                else
                    @assert T <: Union{String,Missing}
                end
                if hasmissing && !(T >: Union{T,Missing})
                    # copy data to a new column
                    col = copyto!(Vector{Union{T,Missing}}(undef, length(col) + n_new_records), 1, col, 1, length(col))
                else
                    # resize the column for new records
                    resize!(col, length(col) + n_new_records)
                end
                fillcolumn!(col, n_new_records, mem, tokens, i, quot)
                columns[i] = col
            end
        end
        skip(stream, pos)
        if !chunking
            @assert eof(stream)
        end
        fillbuffer(stream, eager = true)
    end
    return DataFrame(columns, colnames)
end

# Count the number of lines in a memory block.
function countlines(mem::Memory; byte::UInt8 = LF)
    n = 0
    @inbounds @simd for i in 1:length(mem)
        n += mem[i] == byte
    end
    return n
end

# token kind
const STRING  = 0b0000
const INTEGER = 0b0001
const FLOAT   = 0b0010
const QSTRING = 0b0100  # string with quotation marks
const MISSING = 0b1011  # missing can be any data type

const MAX_TOKEN_START = 2^36
const MAX_TOKEN_LENGTH = 2^24

struct Token
    # From most significant
    #    4bit: kind (+ missing)
    #   36bit: start position (64 GiB)
    #   24bit: length (16 MiB)
    value::UInt64

    function Token(kind::UInt8, start::Int, len::Int)
        @assert start < MAX_TOKEN_START
        @assert len < MAX_TOKEN_LENGTH
        return new((UInt64(kind) << 60) | (UInt64(start) << 24) | UInt64(len))
    end
end

const TOKEN_NULL = Token(0x00, 0, 0)

function kind(token::Token)
    return (token.value >> 60) % UInt8
end

function ismissing(token::Token)
    return (token.value & (UInt64(1) << 63)) != 0
end

function location(token::Token)
    x = token.value & (~UInt64(0) >> 4)
    return (x >> 24) % Int, (x & (~UInt64(0) >> 40)) % Int
end

function aggregate_columns(tokens::Matrix{Token}, nrows::Int)
    ncols = size(tokens, 1)
    bitmaps = Vector{UInt8}(undef, ncols)
    fill!(bitmaps, 0b0111)
    @inbounds for j in 1:nrows, i in 1:ncols
        # Note that the tokens matrix is transposed.
        x = kind(tokens[i,j])
        y = bitmaps[i]
        bitmaps[i] = ((x | y) & 0b1000) | ((x & y) & 0b0111)
    end
    return bitmaps
end

function fillcolumn!(col::Vector{Int}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    @inbounds for i in 1:nvals
        start, length = location(tokens[c,i])
        col[end-nvals+i] = parse_integer(mem, start, length)
    end
    return col
end

function fillcolumn!(col::Vector{Union{Int,Missing}}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    @inbounds for i in 1:nvals
        t = tokens[c,i]
        if ismissing(t)
            col[end-nvals+i] = missing
        else
            start, length = location(t)
            col[end-nvals+i] = parse_integer(mem, start, length)
        end
    end
    return col
end

const SAFE_INT_LENGTH = sizeof(string(typemax(Int))) - 1

@inline function parse_integer(mem::Memory, start::Int, length::Int)
    stop = start + length - 1
    if length > SAFE_INT_LENGTH
        # safe but slow fallback
        buf = IOBuffer()
        for i in start:stop
            write(buf, mem[i])
        end
        return parse(Int, String(take!(buf)))
    end
    i = start
    b = mem[i]
    if b == UInt8('-')
        sign = -1
        i += 1
    elseif b == UInt8('+')
        sign = +1
        i += 1
    else
        sign = +1
    end
    n::Int = 0
    while i ≤ stop
        @inbounds b = mem[i]
        n = 10n + (b - UInt8('0'))
        i += 1
    end
    return sign * n
end

function fillcolumn!(col::Vector{Float64}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    @inbounds for i in 1:nvals
        start, length = location(tokens[c,i])
        col[end-nvals+i] = parse_float(mem, start, length)
    end
    return col
end

function fillcolumn!(col::Vector{Union{Float64,Missing}}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    @inbounds for i in 1:nvals
        t = tokens[c,i]
        if ismissing(t)
            col[end-nvals+i] = missing
        else
            start, length = location(tokens[c,i])
            col[end-nvals+i] = parse_float(mem, start, length)
        end
    end
    return col
end

@inline function parse_float(mem::Memory, start::Int, length::Int)
    return ccall(:strtod, Cdouble, (Ptr{UInt8}, Ptr{Cvoid}), mem.ptr + start - 1, C_NULL)
    #=  The above would be safe and faster.
    hasvalue, val = ccall(:jl_try_substrtod, Tuple{Bool,Float64}, (Ptr{UInt8}, Csize_t, Csize_t), mem.ptr, start-1, length)
    @assert hasvalue
    return val
    =#
end

function fillcolumn!(col::Vector{String}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    last = ""
    @inbounds for i in 1:nvals
        col[end-nvals+i] = last = try_reuse_string(last, mem, tokens[c,i], quot)
    end
    return col
end

function fillcolumn!(col::Vector{Union{String,Missing}}, nvals::Int, mem::Memory, tokens::Matrix{Token}, c::Int, quot::UInt8)
    last = ""
    @inbounds for i in 1:nvals
        t = tokens[c,i]
        if ismissing(t)
            col[end-nvals+i] = missing
        else
            col[end-nvals+i] = last = try_reuse_string(last, mem, t, quot)
        end
    end
    return col
end

@inline function try_reuse_string(last::String, mem::Memory, token::Token, quot::UInt8)
    start, length = location(token)
    if kind(token) & QSTRING != 0
        return qstring(mem, start, length, quot)
    elseif length == sizeof(last) && ccall(:memcmp, Cint, (Ptr{Cvoid}, Ptr{Cvoid}, Csize_t), mem.ptr + start - 1, pointer(last), length) == 0
        return last
    else
        return unsafe_string(mem.ptr + start - 1, length)
    end
end

function qstring(mem::Memory, start::Int, length::Int, quot::UInt8)
    i = start
    skip = false
    buf = IOBuffer()
    while i < start + length
        if skip
            skip = false
        else
            x = mem[i]
            write(buf, x)
            skip = x == quot
        end
        i += 1
    end
    return String(take!(buf))
end

# Read header and return column names.
function readheader(stream::TranscodingStream, delim::UInt8, quot::UInt8, trim::Bool)
    fillbuffer(stream, eager = true)
    mem = buffermem(stream.state.buffer1)
    nl = find_first_newline(mem, 1)
    if nl == 0
        # TODO: maybe, the header is too long to be stored in the memory buffer
        return Symbol[]
    end
    n, tokens = scanheader(mem, 0, nl, delim, quot, trim)
    skip(stream, n)
    colnames = Symbol[]
    for token in tokens
        start, length = location(token)
        if (kind(token) & QSTRING) != 0
            name = qstring(mem, start, length, quot)
        else
            name = unsafe_string(mem.ptr + start - 1, length)
        end
        push!(colnames, Symbol(name))
    end
    return colnames
end

function rename_unnamed_columns!(colnames::Vector{Symbol})
    for i in 1:length(colnames)
        name = colnames[i]
        if name == Symbol("")
            colnames[i] = Symbol("UNNAMED_$(i)")
        end
    end
    return colnames
end

function find_first_newline(mem::Memory, i::Int)
    last = lastindex(mem)
    @inbounds while i ≤ last
        x = mem[i]
        if x == CR
            if i + 1 ≤ last && mem[i+1] == LF
                i += 1
            end
            break
        elseif x == LF
            break
        end
        i += 1
    end
    if i > last
        return 0
    end
    return i
end

function find_last_newline(mem::Memory)
    i = lastindex(mem)
    while i > 0
        @inbounds x = mem[i]
        if x == LF || x == CR
            break
        end
        i -= 1
    end
    return i
end

# Line parser
# -----------

struct ReadError <: Exception
    msg::String
end

macro state(name, ex)
    @assert name isa Symbol
    @assert ex isa Expr && ex.head == :block
    quote
        @label $(name)
        #println($(QuoteNode(name)))
        #@show quoted
        pos += 1
        #if pos > pos_end
        #    @goto END
        #end
        @inbounds c = mem[pos]
        #@show Char(c)
        #println()
        $(ex)
        # This area is unreachable because transition must be exhaustive.
        @assert false
    end |> esc
end

macro multibytestring()
    quote
        # multibyte UTF8 character
        if 0b110_00000 ≤ c ≤ 0b110_11111
            if pos + 1 ≤ pos_end && (mem[pos+1] >> 6) ≤ 0b10  # same as: 0b10_000000 ≤ mem[pos+1] ≤ 0b10_111111
                pos += 1
                @goto STRING
            end
        elseif 0b1110_0000 ≤ c ≤ 0b1110_1111
            if pos + 2 ≤ pos_end && (max(mem[pos+1], mem[pos+2]) >> 6) ≤ 0b10
                pos += 2
                @goto STRING
            end
        elseif 0b11110_000 ≤ c ≤ 0b11110_111
            if pos + 3 ≤ pos_end && (max(mem[pos+1], mem[pos+2], mem[pos+3]) >> 6) ≤ 0b10
                pos += 3
                @goto STRING
            end
        else
            @goto ERROR
        end
    end |> esc
end

macro follows(s)
    @assert s isa String && isascii(s)
    i = 0
    foldl(s, init = :(pos + $(sizeof(s)) ≤ pos_end)) do ex, c
        up = UInt8(uppercase(c))
        lo = UInt8(lowercase(c))
        i += 1
        :($(ex) && (mem[pos+$(i)] == $(up) || mem[pos+$(i)] == $(lo)))
    end |> esc
end

macro begintoken()
    esc(:(start = pos))
end

macro recordtoken(kind)
    quote
        token = Token($(kind), start, pos - start)
    end |> esc
end

macro endheadertoken()
    quote
        push!(tokens, token)
        quoted = false
        qstring = false
    end |> esc
end

function scanheader(mem::Memory, pos::Int, nl::Int, delim::UInt8, quot::UInt8, trim::Bool)
    tokens = Token[]
    token = TOKEN_NULL
    quoted = false
    qstring = false
    start = 0
    pos_end = nl

    @state BEGIN begin
        @begintoken
        if c == quot
            if quoted
                @recordtoken STRING
                @goto QUOTE_END
            end
            quoted = true
            @goto BEGIN
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken STRING
            @endheadertoken
            @goto BEGIN
        elseif c == SP
            if trim && !quoted
                @goto BEGIN
            end
            @goto STRING
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @recordtoken STRING
            @endheadertoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endheadertoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state STRING begin
        if quoted && c == quot
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @endheadertoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                if qstring
                    @recordtoken QSTRING
                else
                    @recordtoken STRING
                end
                @goto STRING_SPACE
            end
            @goto STRING
        elseif c == LF
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @recordtoken STRING
            @endheadertoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endheadertoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state STRING_SPACE begin
        if c == SP
            @goto STRING_SPACE
        elseif c == delim
            @endheadertoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @endheadertoken
            @goto END
        elseif c == CR
            @endheadertoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state QUOTE_END begin
        if c == delim
            @endheadertoken
            @goto BEGIN
        elseif c == quot
            qstring = true
            @goto STRING
        elseif c == SP
            if trim
                @goto QUOTE_END_SPACE
            end
            @goto ERROR
        elseif c == LF
            @endheadertoken
            @goto END
        elseif c == CR
            @endheadertoken
            @goto CR
        else
            @goto ERROR
        end
    end

    @state QUOTE_END_SPACE begin
        if c == delim
            @endheadertoken
            @goto BEGIN
        elseif c == SP
            @goto QUOTE_END_SPACE
        elseif c == LF
            @endheadertoken
            @goto END
        elseif c == CR
            @endheadertoken
            @goto CR
        else
            @goto ERROR
        end
    end

    @label ERROR
    throw(ReadError("invalid file header format"))

    @label CR
    if pos + 1 ≤ pos_end && mem[pos + 1] == LF
        pos += 1
    end

    @label END
    return pos, tokens
end

macro endtoken()
    quote
        if i > ncols
            @goto ERROR
        end
        @inbounds tokens[i,row] = token
        i += 1
        quoted = false
        qstring = false
    end |> esc
end

# Scan a line in mem; mem must include one or more lines.
function scanline!(
        # output info
        tokens::Matrix{Token}, row::Int,
        # input info
        mem::Memory, pos::Int, lastnl::Int, line::Int,
        # parser parameters
        delim::UInt8, quot::UInt8, trim::Bool
    )

    # Check parameters.
    @assert delim != quot
    @assert !trim || delim != SP
    @assert !trim || quot != SP

    # Initialize variables.
    pos_end = lastnl
    ncols = size(tokens, 1)
    quoted = false
    qstring = false
    token = TOKEN_NULL
    start = 0  # the starting position of a token
    i = 1  # the current token

    @state BEGIN begin
        @begintoken
        if c == quot
            if quoted
                @recordtoken MISSING
                @goto QUOTE_END
            else
                quoted = true
                @goto BEGIN
            end
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken MISSING
            @endtoken
            @goto BEGIN
        elseif c == UInt8('-') || c == UInt8('+')
            @goto SIGN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto INTEGER
        elseif c == SP
            if trim && !quoted
                @goto BEGIN
            end
            @goto STRING
        elseif c == UInt8('.')
            @goto DOT
        elseif (c == UInt8('N') || c == UInt8('n')) && @follows("AN")  # case-insensitive
            # NaN
            pos += 2  # for 'A' and 'N'
            @goto SPECIAL_FLOAT
        elseif c == UInt8('I') || c == UInt8('i')
            # Infinity
            if @follows("NFINITY")
                pos += 7
                @goto SPECIAL_FLOAT
            elseif @follows("NF")
                pos += 2
                @goto SPECIAL_FLOAT
            end
            @goto STRING
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            if i == ncols
                @recordtoken MISSING
            end
            @endtoken
            @goto END
        elseif c == CR
            if i == ncols
                @recordtoken MISSING
            end
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state SIGN begin
        if quoted && c == quot
            @recordtoken STRING
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken STRING
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto INTEGER
        elseif c == UInt8('.')
            @goto DOT
        elseif c == SP
            if trim && !quoted
                @recordtoken STRING
                @goto STRING_SPACE
            end
            @goto STRING
        elseif (c == UInt8('N') || c == UInt8('n')) && @follows("AN")
            pos += 2
            @goto SPECIAL_FLOAT
        elseif c == UInt8('I') || c == UInt8('i')
            # Infinity
            if @follows("NFINITY")
                pos += 7
                @goto SPECIAL_FLOAT
            elseif @follows("NF")
                pos += 2
                @goto SPECIAL_FLOAT
            end
            @goto STRING
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @recordtoken STRING
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state INTEGER begin
        if quoted && c == quot
            @recordtoken INTEGER|FLOAT
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken INTEGER|FLOAT
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto INTEGER
        elseif c == UInt8('.')
            @goto POINT_FLOAT
        elseif c == SP
            if trim && !quoted
                @recordtoken INTEGER|FLOAT
                @goto INTEGER_SPACE
            end
            @goto STRING
        elseif c == UInt8('e') || c == UInt8('E')
            @goto EXPONENT
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @recordtoken INTEGER|FLOAT
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken INTEGER|FLOAT
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state INTEGER_SPACE begin
        if c == SP
            @goto INTEGER_SPACE
        elseif c == delim
            @endtoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @endtoken
            @goto END
        elseif c == CR
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state DOT begin
        if quoted && c == quot
            @recordtoken STRING
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken STRING
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto POINT_FLOAT
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                @recordtoken STRING
                @goto STRING_SPACE
            end
            @goto STRING
        elseif c == LF
            @recordtoken STRING
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state POINT_FLOAT begin
        if quoted && c == quot
            @recordtoken FLOAT
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken FLOAT
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto POINT_FLOAT
        elseif c == UInt8('e') || c == UInt8('E')
            @goto EXPONENT
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                @recordtoken FLOAT
                @goto FLOAT_SPACE
            end
            @goto STRING
        elseif c == LF
            @recordtoken FLOAT
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken FLOAT
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state EXPONENT begin
        if quoted && c == quot
            @recordtoken STRING
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken STRING
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto EXPONENT_FLOAT
        elseif c == UInt8('-') || c == UInt8('+')
            @goto EXPONENT_SIGN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                @recordtoken STRING
                @goto STRING_SPACE
            end
            @goto STRING
        elseif c == LF
            @recordtoken STRING
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state EXPONENT_SIGN begin
        if quoted && c == quot
            @recordtoken STRING
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken STRING
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto EXPONENT_FLOAT
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                @recordtoken STRING
                @goto STRING_SPACE
            end
            @goto STRING
        elseif c == LF
            @recordtoken STRING
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken STRING
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state EXPONENT_FLOAT begin
        if quoted && c == quot
            @recordtoken FLOAT
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken FLOAT
            @endtoken
            @goto BEGIN
        elseif UInt8('0') ≤ c ≤ UInt8('9')
            @goto EXPONENT_FLOAT
        elseif c == SP
            if trim && !quoted
                @recordtoken FLOAT
                @goto FLOAT_SPACE
            end
            @goto STRING
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @recordtoken FLOAT
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken FLOAT
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state SPECIAL_FLOAT begin
        if quoted && c == quot
            @recordtoken FLOAT|STRING
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            @recordtoken FLOAT|STRING
            @endtoken
            @goto BEGIN
        elseif c == SP
            if trim && !quoted
                @recordtoken FLOAT|STRING
                @goto FLOAT_SPACE
            end
            @goto STRING
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @recordtoken FLOAT|STRING
            @endtoken
            @goto END
        elseif c == CR
            @recordtoken FLOAT|STRING
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state FLOAT_SPACE begin
        if c == SP
            @goto FLOAT_SPACE
        elseif c == delim
            @endtoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @endtoken
            @goto END
        elseif c == CR
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state STRING begin
        if quoted && c == quot
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @goto QUOTE_END
        elseif c == delim
            if quoted
                @goto STRING
            end
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @endtoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == SP
            if trim && !quoted
                if qstring
                    @recordtoken QSTRING
                else
                    @recordtoken STRING
                end
                @goto STRING_SPACE
            end
            @goto STRING
        elseif c == LF
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @endtoken
            @goto END
        elseif c == CR
            if qstring
                @recordtoken QSTRING
            else
                @recordtoken STRING
            end
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state STRING_SPACE begin
        if c == SP
            @goto STRING_SPACE
        elseif c == delim
            @endtoken
            @goto BEGIN
        elseif UInt8('!') ≤ c ≤ UInt8('~')
            @goto STRING
        elseif c == LF
            @endtoken
            @goto END
        elseif c == CR
            @endtoken
            @goto CR
        else
            @multibytestring
        end
    end

    @state QUOTE_END begin
        if c == delim
            @endtoken
            @goto BEGIN
        elseif c == quot  # e.g. xxx," foo ""bar""",xxx
            qstring = true
            @goto STRING
        elseif c == SP
            if trim
                @goto QUOTE_END_SPACE
            end
            @goto ERROR
        elseif c == LF
            @endtoken
            @goto END
        elseif c == CR
            @endtoken
            @goto CR
        else
            @goto ERROR
        end
    end

    @state QUOTE_END_SPACE begin
        if c == delim
            @endtoken
            @goto BEGIN
        elseif c == SP
            @goto QUOTE_END_SPACE
        elseif c == LF
            @endtoken
            @goto END
        elseif c == CR
            @endtoken
            @goto CR
        else
            @goto ERROR
        end
    end

    @label ERROR
    throw(ReadError("invalid file format at line $(line), char $(repr(c))"))

    @label CR
    if pos + 1 ≤ pos_end && mem[pos + 1] == LF
        pos += 1
    end

    @label END
    if i ≤ ncols
        throw(ReadError("invalid number of columns at line $(line)"))
    end
    return pos
end

# Generated from tools/snoop.jl.
function _precompile_()
    ccall(:jl_generating_output, Cint, ()) == 1 || return nothing
    precompile(Tuple{typeof(TableReader.scanline!), Array{TableReader.Token, 2}, Int64, TranscodingStreams.Memory, Int64, Int64, Int64, UInt8, UInt8, Bool})
    precompile(Tuple{typeof(TableReader.scanheader), TranscodingStreams.Memory, Int64, Int64, UInt8, UInt8, Bool})
    precompile(Tuple{typeof(TableReader.checkformat), Base.IOStream})
    precompile(Tuple{typeof(TableReader.find_first_newline), TranscodingStreams.Memory, Int64})
    precompile(Tuple{typeof(TableReader.check_parser_parameters), Char, Char, Bool, Nothing, Int64})
    precompile(Tuple{typeof(TableReader.checkformat), TranscodingStreams.TranscodingStream{TranscodingStreams.Noop, Base.IOStream}})
    precompile(Tuple{typeof(TableReader.readdlm_internal), TranscodingStreams.TranscodingStream{TranscodingStreams.Noop, Base.IOStream}, TableReader.ParserParameters})
    precompile(Tuple{typeof(TableReader.fillcolumn!), Array{Int64, 1}, Int64, TranscodingStreams.Memory, Array{TableReader.Token, 2}, Int64, UInt8})
    precompile(Tuple{typeof(TableReader.fillcolumn!), Array{String, 1}, Int64, TranscodingStreams.Memory, Array{TableReader.Token, 2}, Int64, UInt8})
    precompile(Tuple{typeof(TableReader.qstring), TranscodingStreams.Memory, Int64, Int64, UInt8})
    precompile(Tuple{typeof(TableReader.aggregate_columns), Array{TableReader.Token, 2}, Int64})
    precompile(Tuple{typeof(TableReader.checkformat), TranscodingStreams.TranscodingStream{TranscodingStreams.Noop, Base.Process}})
    precompile(Tuple{typeof(TableReader.fillcolumn!), Array{Float64, 1}, Int64, TranscodingStreams.Memory, Array{TableReader.Token, 2}, Int64, UInt8})
    precompile(Tuple{typeof(TableReader.wrapstream), Base.IOStream, TableReader.ParserParameters})
    precompile(Tuple{typeof(TableReader.wrapstream), Base.Process, TableReader.ParserParameters})
    precompile(Tuple{typeof(TableReader.checkformat), Base.Process})
    precompile(Tuple{typeof(TableReader.readheader), TranscodingStreams.TranscodingStream{TranscodingStreams.Noop, Base.IOStream}, UInt8, UInt8, Bool})
    precompile(Tuple{typeof(TableReader.readcsv), String})
    precompile(Tuple{typeof(TableReader.readtsv), String})
end
_precompile_()

end # module


text:
using MOCNeutronTransport
using Plots
pyplot()
T = Float64
Nsoln = 20
Nset = [ 1, 2, 3, 4, 5, 10, 15, 20]
f = area
p₁ = Point_2D(T, 0)
p₂ = Point_2D(T, 2)
p₃ = Point_2D(T, 2, 3)
p₄ = Point_2D(T, 0, 3)
p₅ = Point_2D(T, 3//2, 1//2)
p₆ = Point_2D(T, 5//2, 3//2)
p₇ = Point_2D(T, 1//2, 5//2)
p₈ = Point_2D(T, 0,    3//2)
quad8 = Quadrilateral8_2D((p₁, p₂, p₃, p₄, p₅, p₆, p₇, p₈))

soln = f(quad8; N=Nsoln)
err = [abs(soln - f(quad8; N = n)) for n = Nset]
plot(Nset[1:length(Nset)-1] .* Nset[1:length(Nset)-1],
     err[1:length(Nset)-1],
     title = "Quadrature Points vs Quadrilateral8_2D area error (Nsoln = $Nsoln)",
     xaxis=:log,
     yaxis=:log
    )
xlabel!("Quadrature Points")
ylabel!("area error")

ext: jl

max_stars_repo_name: KyleVaughn/MOCNeutronTransport

max_issues_repo_name: KyleVaughn/MOCNeutronTransport

max_forks_repo_name: KyleVaughn/MOCNeutronTransport


text:
"""
    MOI.add_variable(model::Optimizer) = begin

DOCSTRING
"""
MOI.add_variable(model::Optimizer) = VI(variable!(model))
MOI.add_variables(model::Optimizer, n::Int) = [MOI.add_variable(model) for i in 1:n]

MOI.supports_constraint(::Optimizer, ::Type{SVF}) = true

function MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{MOI.EqualTo{T}}
) where {T <: Real}
    return true
end

function MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{MOI.Interval{T}}
) where {T <: Real}
    return true
end

function MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{MOI.LessThan{T}}
) where {T <: Real}
    return true
end

function MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{MOI.GreaterThan{T}}
) where {T <: Real}
    return true
end

MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{DiscreteSet{T}}) where {T <: Number} = true

"""
    MOI.add_constraint(optimizer::Optimizer, v::SVF, set::DiscreteSet{T}) where T <: Number

DOCSTRING

# Arguments:
- `optimizer`: DESCRIPTION
- `v`: DESCRIPTION
- `set`: DESCRIPTION
"""
function MOI.add_constraint(optimizer::Optimizer, v::SVF, set::DiscreteSet{T}
) where {T <: Number}
    vidx = MOI.index_value(v.variable)
    _set_domain!(optimizer, vidx, set.values)
    return CI{SVF,DiscreteSet{T}}(vidx)
end

# function MOI.add_constraint(optimizer::Optimizer, v::SVF, lt::MOI.LessThan{T}
# ) where {T <: AbstractFloat}
#     vidx = MOI.index_value(v.variable)
#     d = make_domain(typemin(Int), lt.upper, Val(:range))
#     update_domain!(optimizer, vidx, d)
#     return CI{SVF,MOI.LessThan{T}}(vidx)
# end

# function MOI.add_constraint(optimizer::Optimizer, v::SVF, gt::MOI.GreaterThan{T}
# ) where {T <: AbstractFloat}
#     vidx = MOI.index_value(v.variable)
#     d = make_domain(gt.lower, typemax(Int), Val(:range))
#     update_domain!(optimizer, vidx, d)
#     return CI{SVF,MOI.GreaterThan{T}}(vidx)
# end

# make_domain(a, b, ::Val{:range}) = domain(Int(a):Int(b))
# make_domain(a, b, ::Val{:inter}) = domain((a, true), (b, true))

function MOI.add_constraint(optimizer::Optimizer, v::SVF, lt::MOI.LessThan{T}
    ) where {T <: AbstractFloat}
        vidx = MOI.index_value(v.variable)
        push!(optimizer.compare_vars, vidx)
        if vidx ∈ optimizer.int_vars
            d = make_domain(typemin(Int), lt.upper, Val(:range))
        else
            a = (Float64(floatmin(Float32)), false)
            b = (lt.upper, true)
            d = make_domain(a, b, Val(:inter))
        end
        update_domain!(optimizer, vidx, d)
        return CI{SVF,MOI.LessThan{T}}(vidx)
    end

    function MOI.add_constraint(optimizer::Optimizer, v::SVF, gt::MOI.GreaterThan{T}
    ) where {T <: AbstractFloat}
        vidx = MOI.index_value(v.variable)
        push!(optimizer.compare_vars, vidx)
        if vidx ∈ optimizer.int_vars
            d = make_domain(gt.lower, typemax(Int), Val(:range))
        else
            a = (gt.lower, true)
            b = (Float64(floatmax(Float32)), false)
            d = make_domain(a, b, Val(:inter))
        end
        update_domain!(optimizer, vidx, d)
        return CI{SVF,MOI.GreaterThan{T}}(vidx)
    end

    make_domain(a, b, ::Val{:range}) = domain(Int(a):Int(b))
    make_domain(a::Real, b::Real, ::Val{:inter}) = domain((a, true), (b, true))
    make_domain(a::Tuple, b::Tuple, ::Val{:inter}) = domain(a, b)

function MOI.add_constraint(optimizer::Optimizer, v::SVF, i::MOI.Interval{T}
) where {T <: Real}
    vidx = MOI.index_value(v.variable)
    is_int = MOI.is_valid(optimizer, CI{SVF, MOI.Integer}(vidx))
    d = make_domain(i.lower, i.upper, Val(is_int ? :range : :inter))
    _set_domain!(optimizer, vidx, d)
    return CI{SVF,MOI.Interval{T}}(vidx)
end

function MOI.add_constraint(optimizer::Optimizer, v::SVF, et::MOI.EqualTo{T}
) where {T <: Number}
    vidx = MOI.index_value(v.variable)
    _set_domain!(optimizer, vidx, et.value)
    return CI{SVF,MOI.EqualTo{T}}(vidx)
end

"""
Binary/Integer variable support
"""
# MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{<:VAR_TYPES}) = true
MOI.supports_constraint(::Optimizer, ::Type{SVF}, ::Type{<:MOI.Integer}) = true

function MOI.add_constraint(optimizer::Optimizer, v::SVF, ::MOI.Integer)
    vidx = MOI.index_value(v.variable)
    push!(optimizer.int_vars, vidx)
    if vidx ∈ optimizer.compare_vars
        x = get_variable(optimizer, vidx)
        _set_domain!(optimizer, vidx, convert(RangeDomain, x.domain))
    end
    return MOI.ConstraintIndex{SVF,MOI.Integer}(vidx)
end


text:
"""
Solves the equations of fluid flow using "Stable Fluids" by Jos Stam with the
FFT to obtain ultra-fast simulations.

Momentum:           ∂u/∂t + (u ⋅ ∇) u = − 1/ρ ∇p + ν ∇²u + f

Incompressibility:  ∇ ⋅ u = 0

u:  Velocity (2d vector)
p:  Pressure
f:  Forcing
ν:  Kinematic Viscosity
ρ:  Density
t:  Time
∇:  Nabla operator (defining nonlinear convection, gradient and divergence)
∇²: Laplace Operator

----

A unit square domain with periodic boundary conditions on each edge
(= simulation on a torus)

          1 +-------------------------------------------------+
            |                                                 |
            |             *                      *            |
            |          *           *    *    *                |
        0.8 |                                                 |
            |                                 *               |
            |     *       *                                   |
            |                      *     *                    |
        0.6 |                                            *    |
            |      *                         <- <- <-         |
            |                             *  <- <- <-         |
            |     -> -> ->               *   <- <- <-         |
            |     -> -> ->    *                *         *    |
        0.4 |     -> -> ->                                    |
            |                                                 |
            |      *            *             *               |
            |           *                             *       |
        0.2 |                       *           *             |
            |                               *                 |
            |  *          *      *                 *       *  |
            |                            *                    |
          0 +-------------------------------------------------+
            0        0.2       0.4       0.6       0.8        1

-> Centered in y but ofset in x there are two horizontal forcings in opposite
   directions

-> Two fluid streams will "collide" with each other

-----

Solution Strategy:

-> Start with zero velocity everywhere: u = [0, 0]

1. Add forces

    w₁ = u + Δt f

2. Convect by self-advection (set the value at the current
   location to be the value at the position backtraced
   on the streamline.) -> unconditionally stable

    w₂ = w₁(p(x, −Δt))

3. Diffuse and Project in Fourier Domain

    3.1 Forward Transformation into Fourier Domain

        w₂ → w₃

    3.2 Diffuse by "low-pass filtering" (convolution
        is multiplication in the Fourier Domain)

        w₄ = exp(− k² ν Δt) w₃

    3.3 Compute the (pseudo-) pressure in the Fourier Domain
        by evaluating the divergence in the Fourier Domain

        q = w₄ ⋅ k / ||k||₂

    3.4 Correct the velocities such that they are incompressible

        w₅ = w₄ − q k / ||k||₂

    3.5 Inverse Transformation back into spatial domain

        w₆ ← w₅

4. Repeat

k = [ kₓ, k_y ] are the spatial frequencies (= wavenumbers)

The Fourier Transformation implicitly prescribes the periodic
Boundary Conditions

------

Changes with respect to the original video (https://youtu.be/F7rWoxeGrko)

1. Use dark theme, set to equal aspect ratio and increase plot window size

2. Change to periodic clamping which respects the periodic boundary conditions,
   then also increase the number of time steps.

"""

using FFTW
using Plots   # Requires ColorSchemes.jl
using ProgressMeter
using Interpolations
using LinearAlgebra

N_POINTS = 250
KINEMATIC_VISCOSITY = 0.0001
TIME_STEP_LENGTH = 0.01
N_TIME_STEPS = 300

function backtrace!(
    backtraced_positions,
    original_positions,
    direction,
)
    # Euler Step backwards in time and periodically clamp into [0.0, 1.0]
    backtraced_positions[:] = mod1.(
        original_positions - TIME_STEP_LENGTH * direction,
        1.0,
    )
end

function interpolate_positions!(
    field_interpolated,
    field,
    interval_x,
    interval_y,
    query_points_x,
    query_points_y,
)
    interpolator = LinearInterpolation(
        (interval_x, interval_y),
        field,
    )
    field_interpolated[:] = interpolator.(query_points_x, query_points_y)
end

function main()
    element_length = 1.0 / (N_POINTS - 1)
    x_interval = 0.0:element_length:1.0
    y_interval = 0.0:element_length:1.0

    # Similar to meshgrid in NumPy
    coordinates_x = [x for x in x_interval, y in y_interval]
    coordinates_y = [y for x in x_interval, y in y_interval]

    wavenumbers_1d = fftfreq(N_POINTS) .* N_POINTS

    wavenumbers_x = [k_x for k_x in wavenumbers_1d, k_y in wavenumbers_1d]
    wavenumbers_y = [k_y for k_x in wavenumbers_1d, k_y in wavenumbers_1d]
    wavenumbers_norm = [norm([k_x, k_y]) for k_x in wavenumbers_1d, k_y in wavenumbers_1d]

    decay = exp.(- TIME_STEP_LENGTH .* KINEMATIC_VISCOSITY .* wavenumbers_norm.^2)

    wavenumbers_norm[iszero.(wavenumbers_norm)] .= 1.0
    normalized_wavenumbers_x = wavenumbers_x ./ wavenumbers_norm
    normalized_wavenumbers_y = wavenumbers_y ./ wavenumbers_norm

    # Define the forces
    force_x = 100.0 .* (
        exp.( - 1.0 / (2 * 0.005) * ((coordinates_x .- 0.2).^2 + (coordinates_y .- 0.45).^2))
        -
        exp.( - 1.0 / (2 * 0.005) * ((coordinates_x .- 0.8).^2 + (coordinates_y .- 0.55).^2))
    )

    # Preallocate all arrays
    backtraced_coordinates_x = zero(coordinates_x)
    backtraced_coordinates_y = zero(coordinates_y)

    velocity_x = zero(coordinates_x)
    velocity_y = zero(coordinates_y)

    velocity_x_prev = zero(velocity_x)
    velocity_y_prev = zero(velocity_y)

    velocity_x_fft = zero(velocity_x)
    velocity_y_fft = zero(velocity_y)
    pressure_fft = zero(coordinates_x)

    # Use a dark theme for plotting
    theme(:dark)

    @showprogress "Timestepping ..." for iter in 1:N_TIME_STEPS

        # (1) Apply the forces
        time_current = (iter - 1) * TIME_STEP_LENGTH
        pre_factor = max(1 - time_current, 0.0)
        velocity_x_prev += TIME_STEP_LENGTH * pre_factor * force_x

        # (2) Self-Advection by backtracing and interpolation
        backtrace!(backtraced_coordinates_x, coordinates_x, velocity_x_prev)
        backtrace!(backtraced_coordinates_y, coordinates_y, velocity_y_prev)
        interpolate_positions!(
            velocity_x,
            velocity_x_prev,
            x_interval,
            y_interval,
            backtraced_coordinates_x,
            backtraced_coordinates_y,
        )
        interpolate_positions!(
            velocity_y,
            velocity_y_prev,
            x_interval,
            y_interval,
            backtraced_coordinates_x,
            backtraced_coordinates_y,
        )

        # (3.1) Transform into Fourier Domain
        velocity_x_fft = fft(velocity_x)
        velocity_y_fft = fft(velocity_y)

        # (3.2) Diffuse by low-pass filtering
        velocity_x_fft .*= decay
        velocity_y_fft .*= decay

        # (3.3) Compute Pseudo-Pressure by Divergence in Fourier Domain
        pressure_fft = (
            velocity_x_fft .* normalized_wavenumbers_x
            +
            velocity_y_fft .* normalized_wavenumbers_y
        )

        # (3.4) Project the velocities to be incompressible
        velocity_x_fft -= pressure_fft .* normalized_wavenumbers_x
        velocity_y_fft -= pressure_fft .* normalized_wavenumbers_y

        # (3.5) Transform back into spatial domain
        velocity_x = real(ifft(velocity_x_fft))
        velocity_y = real(ifft(velocity_y_fft))

        # Advance in time
        velocity_x_prev = velocity_x
        velocity_y_prev = velocity_y

        # Visualize
        d_u__d_y = diff(velocity_x, dims=2)[2:end, :]
        d_v__d_x = diff(velocity_y, dims=1)[:, 2:end]
        curl = d_u__d_y - d_v__d_x
        display(heatmap(x_interval, y_interval, curl', c=:diverging_bkr_55_10_c35_n256, aspect_ratio=:equal, size=(680, 650)))
    end
end

main()


text:
using StaticArrays
using OrdinaryDiffEq
using LinearAlgebra
using Test

@inbounds @inline function ż(z, p, t)
    A, B, D = p
    p₀, p₂ = z[1:2]
    q₀, q₂ = z[3:4]

    return SVector{4}(
        -A * q₀ - 3 * B / √2 * (q₂^2 - q₀^2) - D * q₀ * (q₀^2 + q₂^2),
        -q₂ * (A + 3 * √2 * B * q₀ + D * (q₀^2 + q₂^2)),
        A * p₀,
        A * p₂
  )
end

condition(u, t, integrator) = u
affect!(integrator) = nothing
cbf(idx) = ContinuousCallback(condition,
    affect!, nothing, save_positions=(false, true), idxs=idx)
z0 = SVector{4}(7.1989885061904335, -0.165912283356219, 0., -3.63534900748947)

tspan=(0.,300.)
prob=ODEProblem(ż, z0, tspan, (A=1, B=0.55, D=0.4), callback=cbf(3))
sol=solve(prob, Vern9(), abstol=1e-14, reltol=1e-14,
    save_everystep=false, save_start=false, save_end=false, maxiters=1e6)

@test length(sol) > 100

prob=ODEProblem(ż, z0, (0,400.), (A=1, B=0.55, D=0.4), callback=cbf(3))
sol=solve(prob, Vern9(), abstol=1e-14, reltol=1e-14, save_everystep=false, save_start=false, save_end=false, maxiters=2e4)

@test length(sol) > 100

prob=ODEProblem(ż, z0, (0,5000.), (A=1, B=0.55, D=0.4), callback=cbf(3))
sol=solve(prob, Vern9(), abstol=1e-14, reltol=1e-14, save_everystep=false, save_start=false, save_end=false, maxiters=1e6)

@test length(sol) > 1500

f = function (du,u,p,t)
  du[1] = u[2]
  du[2] = -p[1]
end
function condition(u,t,integrator) # Event when event_f(u,t) == 0
  u[1]
end
function affect!(integrator)
  @test integrator.u[1] >= 0
  integrator.u[2] = -integrator.u[2]
end
cb2 = ContinuousCallback(condition,affect!)
tspan = (0.0,10000.0)
u0 = [50.0,0.0]
p = 9.8
prob = ODEProblem(f,u0,tspan,p)
sol = solve(prob,Tsit5(),callback=cb2)
@test minimum(sol') > -40

function vcondition!(out,u,t,integrator)
  out[1] = u[1]
  out[2] = u[2]
end

function vaffect!(integrator, event_idx)
  @test integrator.u[1] >= 0.0
  if event_idx == 1
    integrator.u[2] = -integrator.u[2]
  else
    integrator.p = 0.0
  end
end

u0 = [50.0,0.0]
tspan = (0.0,15.0)
p = 9.8
prob = ODEProblem(f,u0,tspan,p)
Vcb = VectorContinuousCallback(vcondition!,vaffect!, 2 , save_positions=(true,true))
sol = solve(prob,Tsit5(), callback=Vcb)

f = function (du,u,p,t)
  du[1] = u[2]
  du[2] = -9.81
end

function condition(u,t,integrator) # Event when event_f(u,t) == 0
  u[1]    # Event when height crosses from positive to negative
end

function affect_neg(integrator)
  @test integrator.u[1] >= 0.0
  integrator.u[2] = -0.8*integrator.u[2]
end

cb = ContinuousCallback(condition,nothing,affect_neg! = affect_neg)

u0 = [1.0,0.0]
tspan = (0.0, 3.0)
prob = ODEProblem(f,u0,tspan)
sol = solve(prob,Tsit5(),saveat=0.01,callback=cb)

f! = function (du,u,p,t)
  du[1] = u[2]
  du[2] = -p
end

function condition!(out,u,t,integrator)
  out[1] = u[1]
  out[2] = u[2]
end

function affect!(integrator, event_idx)
  if event_idx == 1
    integrator.u[2] = -integrator.u[2]
  else
    integrator.p = 0.0
  end
end

u0 = [50.0,0.0]
tspan = (0.0,15.0)

begin
    p = 9.8
    prob = ODEProblem(f!,u0,tspan,p)
    Vcb = VectorContinuousCallback(condition!,affect!, 2 , save_positions=(true,true))
    sol = solve(prob,Tsit5(), callback=Vcb)
end

# https://github.com/SciML/DifferentialEquations.jl/issues/758
"""
    eoms(u, p, t)

Equations of motion for the system (6-Dimensional).
"""
function eoms(u, p, t)
    mu = p
    omm = 1 - mu

    r13 = sqrt((u[1] + mu)^2 + u[2]^2 + u[3]^2)
    r13_3 = r13^3
    r23 = sqrt((u[1] - 1 + mu)^2 + u[2]^2 + u[3]^2)
    r23_3 = r23^3

    Ωx = u[1] - (omm) * (u[1] + mu) / r13_3 - mu * (u[1] - omm) / r23_3
    Ωy = u[2] - (omm) * u[2] / r13_3 - mu * u[2] / r23_3
    Ωz = -(omm) * u[3] / r13_3 - mu * u[3] / r23_3

    return @SVector [
        u[4],
        u[5],
        u[6],
        2u[5] + Ωx,
        -2u[4] + Ωy,
        Ωz
    ]
end

"""
    distance(sol, q, t)

Calculate the isochronous position variation between `q` and `sol(t)`.
"""
function distance(sol, q, t)
    posinds = @SVector [1, 2, 3]
    q_sol = sol(t)
    return norm(q[posinds] .- q_sol[posinds])
end

"""
    distance_callback(sol, threshold)

Create a `ContinuousCallback` that terminates integration when `distance` is
less than `threshold`.
"""
function distance_callback(sol, threshold)
    condition(u, t, _) = distance(sol, u, t) - threshold
    affect!(integrator) = nothing
    affect_neg!(integrator) = terminate!(integrator)
    return ContinuousCallback(condition, affect!, affect_neg!)
end

# Mu Parameter
μ = 0.012150607114626023

# State of Chief at time 0
q_chief = @SVector [1.0220277279709828, 0.0, -0.18210117430516676, 0.0, -0.1032699633021813, 0.0]
tof_chief = 10.0

# Integrate the chief trajectory
prob_chief = ODEProblem{false}(eoms, q_chief, (0.0, tof_chief), μ)
sol_chief = solve(prob_chief, Vern9(); abstol=1e-12, reltol=1e-12)

# Define point where deputy departs the chief
t_dep_start = 0.012113772215335508 # Time the deputy trajectory starts at

# Get state of chief at this departure point
q_depart = sol_chief(t_dep_start)

# Apply change in velocity at that point
dv = @SVector [0.0, 0.0, 0.0, 0.00017947635829913735, -5.932764319639683e-5, -0.0009575628577891083]
q_deputy = q_depart + dv

# Build the callback
threshold = 0.00026014568158168577
cb = distance_callback(sol_chief, threshold)

# Build & Solve the deputy problem
tspan_deputy = (t_dep_start, tof_chief - t_dep_start)
prob_deputy = ODEProblem{false}(eoms, q_deputy, tspan_deputy, μ)
sol_deputy = solve(prob_deputy, Vern9(); callback=cb, abstol=1e-12, reltol=1e-12)

gravity = 9.8
stiffness = 500
equilibrium_length = 1
T = 5.0

f2(u, p, t) = begin
    x1, x2, dx1, dx2 = u
    length = abs(x2 - x1)
    spring_force = stiffness * (equilibrium_length - length)
    ddx1 = -gravity - spring_force
    if x1 <= 0
      ddx1 = max(0, ddx1)
    end
    ddx2 = -gravity + spring_force
    [dx1, dx2, ddx1, ddx2]
end

# https://github.com/SciML/DifferentialEquations.jl/issues/647
sol = solve(
    ODEProblem(f, [5.0, 6.0, 0.0, 0.0], (0.0, T)),
    Tsit5(),
    callback = ContinuousCallback((u, _, _) -> u[1], (integrator) -> (integrator.u[3] = 0)),
    reltol = 1e-2,
    abstol = 1e-2
)

# https://github.com/SciML/DifferentialEquations.jl/issues/601
f2(u, p, t) = return [u[2], 0]
prob = ODEProblem(f, [-10., 1.], (-10.0, 10.0), nothing)

is_wall(u, t, integrator) = return u[1]
affect!(integrator) = println("Wall!")
cb = ContinuousCallback(is_wall,
                            affect!
                            )

sol = solve(prob, Tsit5(), callback = cb)

# https://github.com/SciML/DiffEqBase.jl/issues/599

# The actual system of ODEs.  This is a simple system of ODEs here.
function eom!(du,u,params,t)
    issaturated1, issaturated2 = params
    du[1] = u[2] * !Bool(issaturated1)
    du[2] = 3.0 * !any(Bool.([issaturated1, issaturated2]))
    nothing
end

# This condition function is continuously checked throughout
# the whole time-domain simulation.
function condition!(out, u, t, integrator, limits)
    if integrator.p[1] == false            #if state u1 is within saturation limits
        out[1] =  (u[1] - limits[1][2]);   #up crossing of state 1 upper bound
        out[2] = -(u[1] - limits[1][1]);   #up crossing of state 1 lower bound
    else
        out[1] = -1
        out[2] = -1
    end
end

# The "affect!" function below seems to only be called
# when either out[1] or out[2] is zero in the condition! function above.
# That is, when either the state u1 has reached a lower or upper saturation
# limit, this "affect!" function is called.  printing "event_index" will show
# you if out[1] was zero or out[2] was zero.
function affect!(integrator, event_index)
	println("\ninside affect! function..");
    @show event_index
    if event_index ∈ [1,2] # 1 of the 2 sat. limits was met.
        # Set velocity to zero and unsaturate state 2, assumes 0 ∈ bounds of state 2
        println("integrator: ", integrator);
        println("full_cache(integrator): ", full_cache(integrator));
        println("typeof(full_cache(integrator)): ", typeof(full_cache(integrator)));
        println("integrator.p: ", integrator.p);
        println("integrator.u: ", integrator.u);
        println("integrator.du: ", integrator.du);
        println("integrator.t: ", integrator.t);
        #println("size(full_cache(integrator)): ", size(full_cache(integrator)));
        # Set velocity to zero and unsaturate state 2, assumes 0 ∈ bounds of state 2
        for u_ in full_cache(integrator)
            #println("u_: ", u_);
            u_[2] = 0.0;
            #println("u_: ", u_);
        end
        #println("u_: ", u_);
        println("full_cache(integrator): ", full_cache(integrator));
        println("integrator.p: ", integrator.p);
        #integrator.p[2] = false;
        println("integrator.p: ", integrator.p);

        u2 = deepcopy(integrator.u);
        println("u2: ", u2);
        p2 = deepcopy(integrator.p);
        p2[1:2] .= false
        du = similar(u2)
        integrator.f(du,u2,p2,integrator); # this evaluate the integrator ???

        if event_index == 2 # u1 reaches lower limit.
            integrator.p[1] = du[2] > 0.0 ? false : true
        elseif event_index == 1 # u1 reaches upper limit.
            integrator.p[1] = du[2] < 0.0 ? false : true
        end

        println("integrator.p: ", integrator.p);

    end
end

function affect2!(integrator, event_index)
    println("\ninside affect! function..");
    @show event_index
    if event_index ∈ [1,2] # 1 of the 2 sat. limits was met.
        # Set velocity to zero and unsaturate state 2, assumes 0 ∈ bounds of state 2
        println("integrator: ", integrator);
        println("full_cache(integrator): ", full_cache(integrator));
        println("typeof(full_cache(integrator)): ", typeof(full_cache(integrator)));
        println("integrator.p: ", integrator.p);
        println("integrator.u: ", integrator.u);
        println("integrator.du: ", integrator.du);
        println("integrator.t: ", integrator.t);

        integrator.p[2] = false;
        println("integrator.p: ", integrator.p);

        u2 = deepcopy(integrator.u)
        u2[2] = 0.0;
        p2 = deepcopy(integrator.p)
        p2[1:2] .= false
        du = similar(u2)
        integrator.f(du,u2,p2,integrator); # this evaluate the integrator ???

        if event_index == 2 # u1 reaches lower limit.
            integrator.p[1] = du[2] > 0.0 ? false : true
        elseif event_index == 1 # u1 reaches upper limit.
            integrator.p[1] = du[2] < 0.0 ? false : true
        end

        #integrator.p[1] = true;

        println("integrator.p: ", integrator.p);

    end
end

# ----------------------------------------
# The actual simulation is below.
# ----------------------------------------
time_to_sim = 8.0;
u0          = [0.0,-10.0];
tspan       = (0.0,time_to_sim);
data_times  = collect(0.0:1.0e-3:time_to_sim);

# Set saturation limits, compute if IC in limits, and set params accordingly
saturationlimits = [[-10.0,10.0],[-Inf,Inf]];
issaturated = .!([lim[1] <= u_ <= lim[2] for (u_,lim) ∈ zip(u0, saturationlimits)])
params = [issaturated...];

#cb = VectorContinuousCallback((out, u, t, integrator) -> condition!(out, u, t, integrator, saturationlimits),
#                              affect2!,2, affect_neg! = nothing, save_positions=(true,true));

cb = VectorContinuousCallback((out, u, t, integrator) -> condition!(out, u, t, integrator, saturationlimits),
                            affect2!,2, save_positions=(true,true));

prob_sat = ODEProblem(eom!,u0,tspan,params);
sol_nocb = solve(prob_sat,Tsit5(), saveat=data_times);
sol_cb   = solve(prob_sat,Tsit5(), callback = cb, saveat=data_times);


text:
@doc raw"""
    FixedRankMatrices{m,n,k,𝔽} <: Manifold{𝔽}

The manifold of $m × n$ real-valued or complex-valued matrices of fixed rank $k$, i.e.
````math
\bigl\{ p ∈ 𝔽^{m × n}\ \big|\ \operatorname{rank}(p) = k \bigr\},
````
where $𝔽 ∈ \{ℝ,ℂ\}$ and the rank is the number of linearly independent columns of a matrix.

# Representation with 3 matrix factors

A point $p ∈ \mathcal M$ can be stored using unitary matrices $U ∈ 𝔽^{m × k}$, $V ∈ 𝔽^{n × k}$ as well as the $k$
singular values of $p = USV^\mathrm{H}$, where $\cdot^{\mathrm{H}}$ denotes the complex conjugate transpose or
Hermitian. In other words, $U$ and $V$ are from the manifolds [`Stiefel`](@ref)`(m,k,𝔽)` and [`Stiefel`](@ref)`(n,k,𝔽)`,
respectively; see [`SVDMPoint`](@ref) for details.

The tangent space $T_p \mathcal M$ at a point $p ∈ \mathcal M$ with $p=USV^\mathrm{H}$
is given by
````math
T_p\mathcal M = \bigl\{ UMV^\mathrm{T} + U_pV^\mathrm{H} + UV_p^\mathrm{H} :
    M  ∈ 𝔽^{k × k},
    U_p  ∈ 𝔽^{m × k},
    V_p  ∈ 𝔽^{n × k}
    \text{ s.t. }
    U_p^\mathrm{H}U = 0_k,
    V_p^\mathrm{H}V = 0_k
\bigr\},
````
where $0_k$ is the $k × k$ zero matrix. See [`UMVTVector`](@ref) for details.

The (default) metric of this manifold is obtained by restricting the metric
on $ℝ^{m × n}$ to the tangent bundle[^Vandereycken2013].

# Constructor
    FixedRankMatrics(m, n, k[, field=ℝ])

Generate the manifold of `m`-by-`n` (`field`-valued) matrices of rank `k`.

[^Vandereycken2013]:
    > Bart Vandereycken: "Low-rank matrix completion by Riemannian Optimization,
    > SIAM Journal on Optiomoization, 23(2), pp. 1214–1236, 2013.
    > doi: [10.1137/110845768](https://doi.org/10.1137/110845768),
    > arXiv: [1209.3834](https://arxiv.org/abs/1209.3834).
"""
struct FixedRankMatrices{M,N,K,𝔽} <: Manifold{𝔽} end
function FixedRankMatrices(m::Int, n::Int, k::Int, field::AbstractNumbers = ℝ)
    return FixedRankMatrices{m,n,k,field}()
end

@doc raw"""
    SVDMPoint <: MPoint

A point on a certain manifold, where the data is stored in a svd like fashion,
i.e. in the form $USV^\mathrm{H}$, where this structure stores $U$, $S$ and
$V^\mathrm{H}$. The storage might also be shortened to just $k$ singular values
and accordingly shortened $U$ (columns) and $V^\mathrm{T}$ (rows).

# Constructors
* `SVDMPoint(A)` for a matrix `A`, stores its svd factors (i.e. implicitly $k=\min\{m,n\}$)
* `SVDMPoint(S)` for an `SVD` object, stores its svd factors (i.e. implicitly $k=\min\{m,n\}$)
* `SVDMPoint(U,S,Vt)` for the svd factors to initialize the `SVDMPoint`` (i.e. implicitly $k=\min\{m,n\}$)
* `SVDMPoint(A,k)` for a matrix `A`, stores its svd factors shortened to the
  best rank $k$ approximation
* `SVDMPoint(S,k)` for an `SVD` object, stores its svd factors shortened to the
  best rank $k$ approximation
* `SVDMPoint(U,S,Vt,k)` for the svd factors to initialize the `SVDMPoint`,
  stores its svd factors shortened to the best rank $k$ approximation
"""
struct SVDMPoint{TU<:AbstractMatrix,TS<:AbstractVector,TVt<:AbstractMatrix} <: MPoint
    U::TU
    S::TS
    Vt::TVt
end
SVDMPoint(A::AbstractMatrix) = SVDMPoint(svd(A))
SVDMPoint(S::SVD) = SVDMPoint(S.U, S.S, S.Vt)
SVDMPoint(A::Matrix, k::Int) = SVDMPoint(svd(A), k)
SVDMPoint(S::SVD, k::Int) = SVDMPoint(S.U, S.S, S.Vt, k)
SVDMPoint(U, S, Vt, k::Int) = SVDMPoint(U[:, 1:k], S[1:k], Vt[1:k, :])
Base.:(==)(x::SVDMPoint, y::SVDMPoint) = (x.U == y.U) && (x.S == y.S) && (x.Vt == y.Vt)

@doc raw"""
    UMVTVector <: TVector

A tangent vector that can be described as a product $UMV^\mathrm{H}$, at least
together with its base point, see for example [`FixedRankMatrices`](@ref). This
vector structure stores the additionally (to the point) required fields.

# Constructors
* `UMVTVector(U,M,Vt)` store umv factors to initialize the `UMVTVector`
* `UMVTVector(U,M,Vt,k)` store the umv factors after shortening them down to
  inner dimensions $k$, i.e. in $UMV^\mathrm{H}$, where $M$ is a $k × k$ matrix.
"""
struct UMVTVector{TU<:AbstractMatrix,TM<:AbstractMatrix,TVt<:AbstractMatrix} <: TVector
    U::TU
    M::TM
    Vt::TVt
end

UMVTVector(U, M, Vt, k::Int) = UMVTVector(U[:, 1:k], M[1:k, 1:k], Vt[1:k, :])

# here the division in M corrects for the first factor in UMV + x.U*Vt + U*x.Vt, where x is the base point to v.
Base.:*(v::UMVTVector, s::Number) = UMVTVector(v.U * s, v.M * s, v.Vt * s)
Base.:*(s::Number, v::UMVTVector) = UMVTVector(s * v.U, s * v.M, s * v.Vt)
Base.:/(v::UMVTVector, s::Number) = UMVTVector(v.U / s, v.M / s, v.Vt / s)
Base.:\(s::Number, v::UMVTVector) = UMVTVector(s \ v.U, s \ v.M, s \ v.Vt)
Base.:+(v::UMVTVector, w::UMVTVector) = UMVTVector(v.U + w.U, v.M + w.M, v.Vt + w.Vt)
Base.:-(v::UMVTVector, w::UMVTVector) = UMVTVector(v.U - w.U, v.M - w.M, v.Vt - w.Vt)
Base.:-(v::UMVTVector) = UMVTVector(-v.U, -v.M, -v.Vt)
Base.:+(v::UMVTVector) = UMVTVector(v.U, v.M, v.Vt)
Base.:(==)(v::UMVTVector, w::UMVTVector) = (v.U == w.U) && (v.M == w.M) && (v.Vt == w.Vt)

@doc raw"""
    check_manifold_point(M::FixedRankMatrices{m,n,k}, p; kwargs...)

Check whether the matrix or [`SVDMPoint`](@ref) `x` ids a valid point on the
[`FixedRankMatrices`](@ref)`{m,n,k,𝔽}` `M`, i.e. is an `m`-by`n` matrix of
rank `k`. For the [`SVDMPoint`](@ref) the internal representation also has to have the right
shape, i.e. `p.U` and `p.Vt` have to be unitary. The keyword arguments are passed to the
`rank` function that verifies the rank of `p`.
"""
function check_manifold_point(M::FixedRankMatrices{m,n,k}, p; kwargs...) where {m,n,k}
    r = rank(p; kwargs...)
    s = "The point $(p) does not lie on $(M), "
    if size(p) != (m, n)
        return DomainError(size(p), string(s, "since its size is wrong."))
    end
    if r > k
        return DomainError(r, string(s, "since its rank is too large ($(r))."))
    end
    return nothing
end
function check_manifold_point(
    M::FixedRankMatrices{m,n,k},
    x::SVDMPoint;
    kwargs...,
) where {m,n,k}
    s = "The point $(x) does not lie on $(M), "
    if (size(x.U) != (m, k)) || (length(x.S) != k) || (size(x.Vt) != (k, n))
        return DomainError(
            [size(x.U)..., length(x.S), size(x.Vt)...],
            string(
                s,
                "since the dimensions do not fit (expected $(n)x$(m) rank $(k) got $(size(x.U,1))x$(size(x.Vt,2)) rank $(size(x.S)).",
            ),
        )
    end
    if !isapprox(x.U' * x.U, one(zeros(k, k)); kwargs...)
        return DomainError(
            norm(x.U' * x.U - one(zeros(k, k))),
            string(s, " since U is not orthonormal/unitary."),
        )
    end
    if !isapprox(x.Vt * x.Vt', one(zeros(k, k)); kwargs...)
        return DomainError(
            norm(x.Vt * x.Vt' - one(zeros(k, k))),
            string(s, " since V is not orthonormal/unitary."),
        )
    end
    return nothing
end

@doc raw"""
    check_tangent_vector(M:FixedRankMatrices{m,n,k}, p, X; check_base_point = true, kwargs...)

Check whether the tangent [`UMVTVector`](@ref) `X` is from the tangent space of the [`SVDMPoint`](@ref) `p` on the
[`FixedRankMatrices`](@ref) `M`, i.e. that `v.U` and `v.Vt` are (columnwise) orthogonal to `x.U` and `x.Vt`,
respectively, and its dimensions are consistent with `p` and `X.M`, i.e. correspond to `m`-by-`n` matrices of rank `k`.
The optional parameter `check_base_point` indicates, whether to call [`check_manifold_point`](@ref)  for `p`.
"""
function check_tangent_vector(
    M::FixedRankMatrices{m,n,k},
    p::SVDMPoint,
    X::UMVTVector;
    check_base_point = true,
    kwargs...,
) where {m,n,k}
    if check_base_point
        c = check_manifold_point(M, p; kwargs...)
        c === nothing || return c
    end
    if (size(X.U) != (m, k)) || (size(X.Vt) != (k, n)) || (size(X.M) != (k, k))
        return DomainError(
            cat(size(X.U), size(X.M), size(X.Vt), dims = 1),
            "The tangent vector $(X) is not a tangent vector to $(p) on $(M), since matrix dimensions do not agree (expected $(m)x$(k), $(k)x$(k), $(k)x$(n)).",
        )
    end
    if !isapprox(X.U' * p.U, zeros(k, k); kwargs...)
        return DomainError(
            norm(X.U' * p.U - zeros(k, k)),
            "The tangent vector $(X) is not a tangent vector to $(p) on $(M) since v.U'x.U is not zero. ",
        )
    end
    if !isapprox(X.Vt * p.Vt', zeros(k, k); kwargs...)
        return DomainError(
            norm(X.Vt * p.Vt - zeros(k, k)),
            "The tangent vector $(X) is not a tangent vector to $(p) on $(M) since v.V'x.V is not zero.",
        )
    end
    return nothing
end

@doc raw"""
    inner(M::FixedRankMatrices, p::SVDMPoint, X::UMVTVector, Y::UMVTVector)

Compute the inner product of `X` and `Y` in the tangent space of `p` on the [`FixedRankMatrices`](@ref) `M`,
which is inherited from the embedding, i.e. can be computed using `dot` on the elements (`U`, `Vt`, `M`) of `X` and `Y`.
"""
function inner(::FixedRankMatrices, x::SVDMPoint, v::UMVTVector, w::UMVTVector)
    return dot(v.U, w.U) + dot(v.M, w.M) + dot(v.Vt, w.Vt)
end

function Base.isapprox(::FixedRankMatrices, p::SVDMPoint, q::SVDMPoint; kwargs...)
    return isapprox(p.U * Diagonal(p.S) * p.Vt, q.U * Diagonal(q.S) * q.Vt; kwargs...)
end
function Base.isapprox(
    ::FixedRankMatrices,
    p::SVDMPoint,
    X::UMVTVector,
    Y::UMVTVector;
    kwargs...,
)
    return isapprox(
        p.U * X.M * p.Vt + X.U * p.Vt + p.U * X.Vt,
        p.U * Y.M * p.Vt + Y.U * p.Vt + p.U * Y.Vt;
        kwargs...,
    )
end

@doc raw"""
    manifold_dimension(M::FixedRankMatrices{m,n,k,𝔽})

Return the manifold dimension for the `𝔽`-valued [`FixedRankMatrices`](@ref) `M`
of dimension `m`x`n` of rank `k`, namely

````math
\dim(\mathcal M) = k(m + n - k) \dim_ℝ 𝔽,
````

where $\dim_ℝ 𝔽$ is the [`real_dimension`](@ref) of `𝔽`.
"""
function manifold_dimension(::FixedRankMatrices{m,n,k,𝔽}) where {m,n,k,𝔽}
    return (m + n - k) * k * real_dimension(𝔽)
end

@doc raw"""
    project(M, p, A)
    project(M, p, X)

Project the matrix $A ∈ ℝ^{m,n}$ or a [`UMVTVector`](@ref) `X` from the embedding or
another tangent space onto the tangent space at $p$ on the [`FixedRankMatrices`](@ref) `M`,
further decomposing the result into $X=UMV$, i.e. a [`UMVTVector`](@ref).
"""
project(::FixedRankMatrices, ::Any, ::Any)

function project!(::FixedRankMatrices, Y::UMVTVector, p::SVDMPoint, A::AbstractMatrix)
    av = A * (p.Vt')
    uTav = p.U' * av
    aTu = A' * p.U
    Y.M .= uTav
    Y.U .= A * p.Vt' - p.U * uTav
    Y.Vt .= (aTu - p.Vt' * uTav')'
    return Y
end
function project!(M::FixedRankMatrices, Y::UMVTVector, p::SVDMPoint, X::UMVTVector)
    return project!(M, Y, p, X.U * X.M * X.Vt)
end

@doc raw"""
    representation_size(M::FixedRankMatrices{m,n,k})

Return the element size of a point on the [`FixedRankMatrices`](@ref) `M`, i.e.
the size of matrices on this manifold $(m,n)$.
"""
@generated representation_size(::FixedRankMatrices{m,n}) where {m,n} = (m, n)

@doc raw"""
    retract(M, p, X, ::PolarRetraction)

Compute an SVD-based retraction on the [`FixedRankMatrices`](@ref) `M` by computing
````math
    q = U_kS_kV_k^\mathrm{H},
````
where $U_k S_k V_k^\mathrm{H}$ is the shortened singular value decomposition $USV=p+X$,
in the sense that $S_k$ is the diagonal matrix of size $k × k$ with the $k$ largest
singular values and $U$ and $V$ are shortened accordingly.
"""
retract(::FixedRankMatrices, ::Any, ::Any, ::PolarRetraction)

function retract!(
    ::FixedRankMatrices{M,N,k},
    q::SVDMPoint,
    p::SVDMPoint,
    X::UMVTVector,
    ::PolarRetraction,
) where {M,N,k}
    s = svd(p.U * Diagonal(p.S) * p.Vt + (p.U * X.M * p.Vt + X.U * p.Vt + X.U * X.Vt))
    q.U .= s.U[:, 1:k]
    q.S .= s.S[1:k]
    q.Vt .= s.Vt[1:k, :]
    return q
end

function Base.show(io::IO, ::FixedRankMatrices{M,N,K,𝔽}) where {M,N,K,𝔽}
    return print(io, "FixedRankMatrices($(M), $(N), $(K), $(𝔽))")
end
function Base.show(io::IO, mime::MIME"text/plain", p::SVDMPoint)
    pre = " "
    summary(io, p)
    println(io, "\nU factor:")
    su = sprint(show, "text/plain", p.U; context = io, sizehint = 0)
    su = replace(su, '\n' => "\n$(pre)")
    println(io, pre, su)
    println(io, "singular values:")
    ss = sprint(show, "text/plain", p.S; context = io, sizehint = 0)
    ss = replace(ss, '\n' => "\n$(pre)")
    println(io, pre, ss)
    println(io, "Vt factor:")
    sv = sprint(show, "text/plain", p.Vt; context = io, sizehint = 0)
    sv = replace(sv, '\n' => "\n$(pre)")
    return print(io, pre, sv)
end
function Base.show(io::IO, mime::MIME"text/plain", X::UMVTVector)
    pre = " "
    summary(io, X)
    println(io, "\nU factor:")
    su = sprint(show, "text/plain", X.U; context = io, sizehint = 0)
    su = replace(su, '\n' => "\n$(pre)")
    println(io, pre, su)
    println(io, "M factor:")
    sm = sprint(show, "text/plain", X.M; context = io, sizehint = 0)
    sm = replace(sm, '\n' => "\n$(pre)")
    println(io, pre, sm)
    println(io, "Vt factor:")
    sv = sprint(show, "text/plain", X.Vt; context = io, sizehint = 0)
    sv = replace(sv, '\n' => "\n$(pre)")
    return print(io, pre, sv)
end

allocate(p::SVDMPoint) = SVDMPoint(allocate(p.U), allocate(p.S), allocate(p.Vt))
function allocate(p::SVDMPoint, ::Type{T}) where {T}
    return SVDMPoint(allocate(p.U, T), allocate(p.S, T), allocate(p.Vt, T))
end
allocate(X::UMVTVector) = UMVTVector(allocate(X.U), allocate(X.M), allocate(X.Vt))
function allocate(X::UMVTVector, ::Type{T}) where {T}
    return UMVTVector(allocate(X.U, T), allocate(X.M, T), allocate(X.Vt, T))
end

function number_eltype(p::SVDMPoint)
    return typeof(one(eltype(p.U)) + one(eltype(p.S)) + one(eltype(p.Vt)))
end
function number_eltype(X::UMVTVector)
    return typeof(one(eltype(X.U)) + one(eltype(X.M)) + one(eltype(X.Vt)))
end

function Base.one(p::SVDMPoint)
    return SVDMPoint(
        one(zeros(size(p.U, 1), size(p.U, 1))),
        ones(length(p.S)),
        one(zeros(size(p.Vt, 2), size(p.Vt, 2))),
        length(p.S),
    )
end
function Base.one(X::UMVTVector)
    return UMVTVector(
        one(zeros(size(X.U, 1), size(X.U, 1))),
        one(zeros(size(X.M))),
        one(zeros(size(X.Vt, 2), size(X.Vt, 2))),
        size(X.M, 1),
    )
end

function Base.copyto!(p::SVDMPoint, q::SVDMPoint)
    copyto!(p.U, q.U)
    copyto!(p.S, q.S)
    copyto!(p.Vt, q.Vt)
    return p
end
function Base.copyto!(X::UMVTVector, Y::UMVTVector)
    copyto!(X.U, Y.U)
    copyto!(X.M, Y.M)
    copyto!(X.Vt, Y.Vt)
    return X
end

@doc raw"""
    zero_tangent_vector(M::FixedRankMatrices, p::SVDMPoint)

Return a [`UMVTVector`](@ref) representing the zero tangent vector in the tangent space of
`p` on the [`FixedRankMatrices`](@ref) `M`, for example all three elements of the resulting
structure are zero matrices.
"""
function zero_tangent_vector(::FixedRankMatrices{m,n,k}, p::SVDMPoint) where {m,n,k}
    v = UMVTVector(
        zeros(eltype(p.U), m, k),
        zeros(eltype(p.S), k, k),
        zeros(eltype(p.Vt), k, n),
    )
    return v
end

function zero_tangent_vector!(
    ::FixedRankMatrices{m,n,k},
    X::UMVTVector,
    p::SVDMPoint,
) where {m,n,k}
    X.U .= zeros(eltype(X.U), m, k)
    X.M .= zeros(eltype(X.M), k, k)
    X.Vt .= zeros(eltype(X.Vt), k, n)
    return X
end


text:
using AppConf, Base.Test

@dev x = "dev"
@prod x = "prod"
@test x == "dev"

ENV["VAR"] = "foo"

parseconf("sample.conf")

@dev x = "dev"
@prod x = "prod"
@test x == "prod"

@test conf["test.param"] == "foo=bar"
@test conf["baz.dot"] == "bar"
@test conf["num"] == 34
@test conf["bool"] == false
@test conf["pi"] == 3.14
@test conf["ip"] == "192.168.31.12"

@test length(conf["arr"]) == 4
@test conf["arr"][1] == "foo"
@test conf["arr"][2] == "bar"
@test conf["arr"][4] == 7

@test conf["vals"][1] == "one"
@test conf["vals"][2] == 2
@test conf["vals"][3] == "three"

#tuples
@test conf["tvals"][1] == "one"
@test conf["tvals"][2] == 2
@test conf["tvals"][3] == "three"

@test conf["white.space"] == ["Hi", 42, "Hello", 111]

@test conf["var"] == "foo/bar"
@test conf["ρ"] == 4
@test conf["γ"] == 1.667
@test conf["V₁"] == 3

@test parseconf("foo.conf")["template"] == true

@test_throws ErrorException parseconf("bar.conf")


text:
using Plots
using TrajectoryOptimization
function comparison_plot(results,sol_ipopt;kwargs...)
    n = size(results.X,1)
    if n == 2 # pendulum
        state_labels = ["pos" "vel"]
    else
        state_labels = ["pos" "angle"]
    end
    time_dircol = linspace(0,obj.tf,size(sol_ipopt.X,2))
    time_ilqr = linspace(0,obj.tf,size(results.X,2))

    colors = [:blue :red]

    p_u = plot(time_ilqr,results.U',width=2,label="iLQR",color=colors,width=2)
    # plot!(time_dircol,sol_snopt.U',width=2, label="Snopt",color=colors,width=2,style=:dash)
    plot!(time_dircol,sol_ipopt.U',width=2, label="Ipopt",color=colors,width=2,style=:dashdot,ylabel="control")

    p_x = plot(time_ilqr,results.X[1:2,:]',width=2,label=state_labels,color=colors,width=2)
    # plot!(time_dircol,sol_snopt.X[1:2,:]',width=2, label="",color=colors,width=2,style=:dash)
    plot!(time_dircol,sol_ipopt.X[1:2,:]',width=2, label="",color=colors,width=2,style=:dashdot,ylabel="state")

    p = plot(p_x,p_u,layout=(2,1),xlabel="time (s)"; kwargs...)

    # p_x = plot(time_dircol,X_dircol[1:2,:]',width=2, label=state_labels.*" (DIRCOL)",color=colors)
    # plot!(time_ilqr,results.X[1:2,:]',width=2,label=state_labels.*" (iLQR)",color=colors, style=:dash,
    #     xlabel="time (s)", ylabel="States",legend=:topleft;kwargs...)
    return p
end

function ilqr_dircol_comparison(model, obj, method, name, X0=Array{Float64}(0,0);
    dt=0.01,mesh=[0.1,0.05,0.03,0.02],
    opts=SolverOptions(verbose=false,
                       eps_constraint=1e-6,
                       eps_intermediate=1e-4,
                       eps=1e-8))

    name *= "_" * string(method)

    # Initial state
    N,dt = TrajectoryOptimization.calc_N(obj.tf, dt)
    U = ones(1,N)*1
    X = zeros(model.n,N)
    if isempty(X0)
        X0 = line_trajectory(obj.x0, obj.xf, N)
    end

    # Feed iLQR to DIRCOL
    println("\niLQR → DIRCOL")
    solver = Solver(model,obj,dt=dt,integration=method,opts=opts)

    println("Solving iLQR...")
    time_i1 = @elapsed results, stats_i1 = solve(solver,X0,U)
    cost_i1 = cost(solver,results)

    println("Solving DIRCOL...")
    time_d1 = @elapsed sol, f_opt, stats_d1, objval = solve_dircol(solver, results.X, results.U, nlp=:ipopt, grads=:none, start=:warm)
    cost_d1 = cost(solver,sol)

    p1 = comparison_plot(results,sol,title=method)
    traj_diff = sol.X-results.X
    max_err1 = maximum(abs.(traj_diff))
    norm_err1 = vecnorm(traj_diff)

    # Feed DIRCOL to iLQR
    println("\nDIRCOL → iLQR")
    solver = Solver(model,obj,dt=dt,integration=method,opts=opts)

    println("Solving DIRCOL...")
    time_d2 = @elapsed sol2, f_opt, stats_d2, objval2 = solve_dircol(solver, X0, U, nlp=:ipopt, grads=:none)
    cost_d2 = cost(solver,sol2)

    println("Solving iLQR...")
    time_i2 = @elapsed results2, stats_i2 = solve(solver,Array(sol2.X),Array(sol2.U))
    cost_i2 = cost(solver,results2)

    p2 = comparison_plot(results2,sol2,title=method)
    traj_diff = sol2.X-results2.X
    max_err2 = maximum(abs.(traj_diff))
    norm_err2 = vecnorm(traj_diff)

    # File output
    println("Output to file...")
    folder = "logs"
    f = open(joinpath(folder,name * ".txt"),"w")
    col = [20,25,25]  # column widths
    create_row(s1,s2,s3) = join([rpad(s1,col[1]),rpad(s2,col[2]), rpad(s3,col[3])]," | ")
    header = create_row("\nStat","iLQR → DIRCOL","DIRCOL → iLQR")

    print_solver(solver,name,f)
    println(f,"\t final tolerance: $(solver.opts.eps)")
    println(f,"\t intermediate tolerance: $(solver.opts.eps_intermediate)")
    println(f,"\t constraint tolerance: $(solver.opts.eps_constraint)")
    println(f,header)
    println(f,repeat("-",sum(col)+3*3))
    println(f,create_row("iLQR runtime (sec)", time_i1, time_i2))
    println(f,create_row("DIRCOL runtime (sec)", time_d1, time_d2))
    println(f,create_row("iLQR cost", cost_i1, cost_i2))
    println(f,create_row("DIRCOL cost", cost_d1, cost_d2))
    println(f,create_row("iLQR iterations", stats_i1["iterations"], stats_i2["iterations"]))
    println(f,create_row("DIRCOL iterations", stats_d1["iterations"], stats_d2["iterations"]))
    println(f,create_row("Max error", max_err1, max_err2))
    println(f,create_row("Norm error", norm_err1, norm_err2))

    savefig(p1,joinpath(folder,name * "_iLQR_to_DIRCOL"))
    savefig(p2,joinpath(folder,name * "_DIRCOL_to_iLQR"))
    close(f)
end

#***************#
#   PENDULUM    #
#***************#

model = Model(Dynamics.pendulum_dynamics!,2,1)
obj0 = Dynamics.pendulum[2]
# model, obj = Dynamics.pendulum!
obj = copy(obj0)
obj.tf = 3.
obj.R .= [1e-2]
obj = ConstrainedObjective(obj)

# ilqr_dircol_comparison(model,obj,:midpoint,"Pendulum")
# ilqr_dircol_comparison(model,obj,:rk3_foh,"Pendulum")

#***************#
#   CART POLE   #
#***************#
model, obj0 = Dynamics.cartpole_analytical

obj = copy(obj0)
obj.x0 = [0;0;0;0.]
obj.xf = [0.5;pi;0;0]
obj.tf = 2.

ilqr_dircol_comparison(model,obj,:midpoint,"Cartpole")
ilqr_dircol_comparison(model,obj,:rk3_foh,"Cartpole")

#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#
#                                                                              #
#                            CONSTRAINTS                                       #
#                                                                              #
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

# Set Solver Options
opts.verbose = true
opts.verbose = false
opts.eps_constraint = 1e-8
opts.eps_intermediate = 1e-6
opts.eps = 1e-8
mesh = [0.1,0.07,0.05]
dt = 0.03

#***************#
#   PENDULUM    #
#***************#

model = Model(Dynamics.pendulum_dynamics!,2,1)
obj0 = Dynamics.pendulum[2]
# model, obj = Dynamics.pendulum!
obj_uncon = copy(obj0)
obj_uncon.tf = 5.
obj_uncon.R .= [1e-1]

u_bnd = 2
obj = ConstrainedObjective(obj_uncon,u_max=u_bnd,u_min=-u_bnd)
name = "Pendulum"
method=:midpoint
time = 0:dt:obj.tf
X0 = -[sin.(time*3) cos.(time*3)]'

ilqr_dircol_comparison(model,obj,:midpoint,"Pendulum (Constrained)",X0,dt=dt,mesh=mesh,opts=opts)
ilqr_dircol_comparison(model,obj,:rk3_foh,"Pendulum (Constrained)",X0,dt=dt,mesh=mesh,opts=opts)

#***************#
#   CART POLE   #
#***************#
model, obj0 = Dynamics.cartpole_analytical
obj = copy(obj0)

obj.x0 = [0;0;0;0.]
obj.xf = [0.5;pi;0;0]
obj.tf = 2.
u_bnd = 30
x_bnd = [1,Inf,Inf,Inf]
dt = 0.01
obj_con = ConstrainedObjective(obj,u_min=-u_bnd, u_max=u_bnd, x_min=-x_bnd, x_max=x_bnd);
method = :rk3

name = "Cartpole"
method = :rk3
dt = 0.01
X0 = []
opts = SolverOptions(verbose=false,
                   eps_constraint=1e-6,
                   eps_intermediate=1e-4,
                   eps=1e-6,
                   outer_loop_update_type=:uniform,
                   constraint_decrease_ratio=0.1)

opts0 = SolverOptions()
name *= "_" * string(method)

# Initial state
N,dt = TrajectoryOptimization.calc_N(obj.tf, dt)
U = ones(1,N)*1
X = zeros(model.n,N)
if isempty(X0)
    X0 = line_trajectory(obj.x0, obj.xf, N)
end

# Feed iLQR to DIRCOL
println("\niLQR → DIRCOL")
solver = Solver(model,obj,dt=dt,integration=method,opts=opts)

println("Solving iLQR...")
solver.opts.verbose = true
time_i1 = @elapsed results, stats_i1 = solve(solver,X0,U)
cost_i1 = cost(solver,results)

println("Solving DIRCOL...")
time_d1 = @elapsed sol_s1, f_opt, stats_d1 = solve_dircol(solver, X0, U, nlp=:snopt, method=:hermite_simpson, grads=:auto, start=:warm)
cost_d1 = cost(solver,sol_s1)

time_d1 = @elapsed sol_i1, f_opt, stats_d1 = solve_dircol(solver, X0, U, nlp=:ipopt, method=:hermite_simpson, grads=:auto, start=:warm)
cost_d1 = cost(solver,sol_i1)

p1 = comparison_plot(results,sol_s1,sol_i1,title=method)
traj_diff = sol_i1.X-results.X
max_err1 = maximum(abs.(traj_diff))
norm_err1 = vecnorm(traj_diff)


text:
const max_methods = 4  # maximum number of MethodInstances tracked for a particular :call statement

"""
    framecode, lenv = get_call_framecode(fargs, parentframe::FrameCode, idx::Int)

Return the framecode and environment for a call specified by `fargs = [f, args...]` (see [`prepare_args`](@ref)).
`parentframecode` is the caller, and `idx` is the program-counter index.
If possible, `framecode` will be looked up from the local method tables of `parentframe`.
"""
function get_call_framecode(fargs, parentframe::FrameCode, idx::Int; enter_generated::Bool=false)
    nargs = length(fargs)  # includes f as the first "argument"
    # Determine whether we can look up the appropriate framecode in the local method table
    if isassigned(parentframe.methodtables, idx)  # if this is the first call, this may not yet be set
        tme = tme1 = parentframe.methodtables[idx]
        local tmeprev
        depth = 1
        while true
            # TODO: consider using world age bounds to handle cache invalidation
            # Determine whether the argument types match the signature
            sig = tme.sig.parameters::SimpleVector
            if length(sig) == nargs
                # If this is generated, match only if `enter_generated` also matches
                mi = tme.func::FrameInstance
                matches = !is_generated(scopeof(mi.framecode)) || enter_generated == mi.enter_generated
                if matches
                    for i = 1:nargs
                        if !isa(fargs[i], sig[i])
                            matches = false
                            break
                        end
                    end
                end
                if matches
                    # Rearrange the list to place this method first
                    # (if we're in a loop, we'll likely match this one again on the next iteration)
                    if depth > 1
                        parentframe.methodtables[idx] = tme
                        tmeprev.next = tme.next
                        tme.next = tme1
                    end
                    return mi.framecode, mi.sparam_vals
                end
            end
            depth += 1
            tmeprev = tme
            tme = tme.next
            tme === nothing && break
            tme = tme::TypeMapEntry
        end
    end
    # We haven't yet encountered this argtype combination and need to look it up by dispatch
    fargs[1] = f = to_function(fargs[1])
    ret = prepare_call(f, fargs; enter_generated=enter_generated)
    ret === nothing && return f(fargs[2:end]...), nothing
    isa(ret, Compiled) && return ret, nothing
    framecode, args, env, argtypes = ret
    # Store the results of the method lookup in the local method table
    mi = FrameInstance(framecode, env, is_generated(scopeof(framecode)) & enter_generated)
    # it's sort of odd to call this a TypeMapEntry, then set most of the fields incorrectly
    # but since we're just using it as a linked list, it's probably ok
    tme = ccall(:jl_new_struct_uninit, Any, (Any,), TypeMapEntry)::TypeMapEntry
    tme.func = mi
    tme.simplesig = nothing
    tme.sig = argtypes
    tme.isleafsig = true
    tme.issimplesig = false
    method = framecode.scope::Method
    tme.va = method.isva
    if isassigned(parentframe.methodtables, idx)
        tme.next = parentframe.methodtables[idx]
        # Drop the oldest tme, if necessary
        tmetmp = tme.next
        depth = 2
        while isdefined(tmetmp, :next) && tmetmp.next !== nothing
            depth += 1
            tmetmp = tmetmp.next
            depth >= max_methods && break
        end
        if depth >= max_methods
            tmetmp.next = nothing
        end
    else
        tme.next = nothing
    end
    parentframe.methodtables[idx] = tme
    return framecode, env
end


text:
# -----------------------------------------------------------
#
# Evaluating the 1D Galerkin Basis functions
#
# -----------------------------------------------------------

const K_max = 10;

# Efficiency criticality: HIGH
# Central to the efficiency of the package

# Performing fastmath on an array of coefficients
# of x^k and |x|^k, giving the value at a specified point
# supported on [-1, 1]
@fastmath function array2poly{T<:Real}(v::Array{T,1}, x::Real)
	if abs(x)>1
		return zero(T)
	end
	n = length(v)
	k = div(n,2)
	s = zero(T)
	xi = one(T)
	sgn = sign(x)
	@inbounds for i in k:-1:1
		# Using Horner's method
		s *= x
		s += v[i] + flipsign(v[i+k], x)#v[i+k]*sign(x)#
	end
	return s
end

function array2poly{T<:Real}(v::Array{T,1})
	return (x-> array2poly(v,x))
end

#precompute Legendre
leg_coeffs = legendre(K_max)

# Legendre polynomial supported on [-1, 1]
function LegendreP(k, x)
	k<=K_max || throw(DomainError())
	return array2poly(leg_coeffs[k+1], x)
end

function LegendreP(k)
	k<=K_max || throw(DomainError())
	return array2poly(leg_coeffs[k+1])
end

# precomputing the DG functions
# TODO Make this a 2D array:
dg_coeffs = Array{Array{Array{Float64,1},1}}(K_max)

for i in 1:K_max
	dg_coeffs[i] = dg_basis(i)
end

# This is the dg basis function corresponding a given mode
# supported on [-1, 1]
function h(k, mode, x)
	mode<=k || throw(DomainError())
	return array2poly((dg_coeffs[k])[mode],x)
end

function h(k, mode)
	mode<=k || throw(DomainError())
	return array2poly((dg_coeffs[k])[mode])
end

# ----------------------------------------------
#
# Methods relating to the 1D Position Basis:
#
# ----------------------------------------------

# Legendre polynomial on [0, 1]
function leg{T<:Real}(mode::Int, x::T)
	return sqrt(2.0)*LegendreP(mode-1, 2*x-1)
end

# level >= 1
# cell in 1:1<<level
# mode in 1:k
# Legendre polynomial on [(cell-1)*h, cell*h] with h = 1/(1<<level)
function basis{T<:Real}(level::Int, cell::Int, mode::Int, x::T)
	return leg(mode, (1<<level)*x - (cell-1)) * (2.0)^(level/2)
end

function basis(level::Int, cell::Int, mode::Int)
	return x->basis(level, cell, mode, x)
end

# Perform numerical integration to get position basis coeffs
#
# Never used except if user demands to evolve wave equation
# using only the position basis, never going through heir
function pos_vcoeffs_DG(k::Int, level::Int, f::Function;
						rel_tol = REL_TOL, abs_tol = ABS_TOL, max_evals = MAX_EVALS)
	vcoeffs = Array{Float64}((1<<level)*(k))
	i = 1
	for cell in 1:(1<<level)
		for mode in 1:k
			fcn			= x->(basis(level, cell, mode,x)*f(x))
			left_bound  = (cell-1)/(1<<level)
			right_bound = (cell)/(1<<level)
			vcoeffs[i]  = hquadrature(fcn, left_bound, right_bound; abstol=abs_tol)[1]
			i += 1
		end
	end
	return vcoeffs
end

# ----------------------------------------------
# Building the hier2pos matrix:
# ----------------------------------------------

# Given an array of the type above, of coefficients
# for both both x^k and |x|^k, then given a choice of
# side (right vs. left) this becomes just a pure
# polynomial array of half the length
function convert_polyarray{T<:Real}(v::Array{T,1}, side="left")
    n = Int(length(v)//2)
	if side == "left"
		return [v[i]-v[n+i] for i in 1:n]
	elseif side == "right"
		[v[i]+v[n+i] for i in 1:n]
	else throw(ArgumentError())
	end
end

# Gives the polynomial array corresponding to p(x/c)
function scale_polyarray{T<:Real}(v::Array{T,1}, c::Real)
	n = length(v)
	return [v[i] * (1//c^(i-1)) for i in 1:n]
end

# Gives the polynomial array corresponding to p(x-a)
function shift_polyarray{T<:Real}(v::Array{T,1}, a::Real)
	n = length(v)
	v_new = zeros(v)
	for i::Int in 1:n
		for j::Int in 1:i
			v_new[j] += v[i] * (-a)^(i-j) * binomial(i-1, j-1)
		end
	end
	return v_new
end

# Performs symbolic integration of the polynomials
# represented by v and w on the interval [a, b]
function integrate_polyarray{T<:Real}(v::Array{T,1}, w::Array{T,1}; a::Real=0, b::Real=1)
	ans = zero(T)
	for i::Int in 1:length(v)
		for j::Int in 1:length(w)
			ans += v[i] * w[j] * ((b^(i+j-1)-a^(i+j-1))//(i+j-1))
		end
	end
	return ans
end

# With a bit of case work, we can do an explicit
# symbolic integration of the elements in the
# hierarchical basis against the legendre polynomials
# defining the position basis:
function hier2pos(k::Int, max_level::Int, level::Int, cell::Int, mode::Int)

	ans = Real[]

	# Compute the support and discontinuity
	# of the hierarchical basis element
	left_point  = (cell-1)//(1<<max(0, level-1))
	mid_point   = (cell- 1//2)//(1<<max(0, level-1))
	right_point = cell//(1<<max(0, level-1))

	# Level 0 of the hierarchical basis is a continous
	# Legendre polynomial
	if level == 0
		vl = vr = sqrt(2)*shift_polyarray(
								scale_polyarray(leg_coeffs[mode][1:k],
												1//2),
											1//2)

	# The higher DG levels are given by two polynomials
	# defined on neighboring (right & left) intervals of size h/2,
	# which would need to be integrated against individually
	else
		h = 1//(1<<max(0, level-1))
		vl = shift_polyarray(
				scale_polyarray(
					convert_polyarray(dg_coeffs[k][mode], "left"),
								1//2),
							1//2)
		vl = shift_polyarray(scale_polyarray(vl, h), (cell-1)*h)*sqrt(1<<(level))

		vr = shift_polyarray(
				scale_polyarray(
					convert_polyarray(dg_coeffs[k][mode], "right"),
					 			1//2),
							1//2)
		vr = shift_polyarray(scale_polyarray(vr, h), (cell-1)*h)*sqrt(1<<(level))
	end

	h = 1//(1<<max_level)
	for i in 1:1<<max_level
		for p in 1:k
			# Check if our hier function is supported on this interval
			if (i-1)*h < left_point || i*h > right_point
				push!(ans, 0)
				continue
			end

			# Obtain the polynomial array for the position basis function
			unit_leg = shift_polyarray(
							scale_polyarray(leg_coeffs[p][1:k], 1//2), 1//2)
			pos_element = shift_polyarray(
								scale_polyarray(unit_leg, h), (i-1)*h)
			pos_element *= sqrt(1<<(max_level+1)) # (2.0)^((max_level+1)/2)

			# Perform symbolic integration depending on which
			# interval the position basis function overlaps with the
			# DG basis element
			if (i-1)*h < mid_point
				val = integrate_polyarray(vl, pos_element; a=(i-1)*h, b=i*h)
			else
				val = integrate_polyarray(vr, pos_element; a=(i-1)*h, b=i*h)
			end
			push!(ans, val)
		end
	end
	return ans
end

# Construct the change of basis matrix from hierarchical
# to position basis
function hier2pos(k::Int, max_level::Int; abs_tol=ABS_TOL)
	j = 1
	I = Int[]
	J = Int[]
	V = Float64[]
	for level in 0:max_level
		for cell in 1:(1<<max(0, level-1))
			for mode in 1:k
				ans = pos_vcoeffs_DG(k, max_level, v(k, level, cell, mode))
				for i in 1:length(ans)
					if abs(ans[i]) > abs_tol
						push!(I, i)
						push!(J, j)
						push!(V, ans[i])
					end
				end
				j += 1
			end
		end
	end
	return sparse(I, J, V, k * (1<<max_level), k * (1<<max_level), +)
end

function pos2hier(k::Int, max_level::Int; abs_tol=ABS_TOL)
	return hier2pos(k, max_level; abs_tol=abs_tol)'
end