text:
# tensorcontract.jl
#
# Method for contracting two tensors and adding the result
# to a third tensor, according to the specified labels.

# Buffer
#--------
immutable TCBuffer
    Abuf::Vector{Uint8}
    Bbuf::Vector{Uint8}
    Cbuf::Vector{Uint8}
end

const defaultcontractbuffer=TCBuffer(Array(Uint8, 1<<13),Array(Uint8, 1<<13),Array(Uint8, 1<<13))

function reset_tcbuffer(size::Int=3*(1<<13))
    singlesize=div(size,3)
    resize!(defaultcontractbuffer.Abuf,singlesize)
    resize!(defaultcontractbuffer.Bbuf,singlesize)
    resize!(defaultcontractbuffer.Cbuf,singlesize)
end

# Simple method
#---------------
function tensorcontract(A::StridedArray,labelsA,B::StridedArray,labelsB,outputlabels=symdiff(labelsA,labelsB);method::Symbol=:BLAS,buffer::TCBuffer=defaultcontractbuffer)
    dimsA=size(A)
    dimsB=size(B)
    dimsC=tuple(dimsA...,dimsB...)
    dimsC=dimsC[indexin(outputlabels,vcat(labelsA,labelsB))]
    T=promote_type(eltype(A),eltype(B))
    C=similar(A,T,dimsC)
    tensorcontract!(1,A,labelsA,'N',B,labelsB,'N',0,C,outputlabels;method=method,buffer=buffer)
end

# In-place method
#-----------------
function tensorcontract!(alpha::Number,A::StridedArray,labelsA,conjA::Char,B::StridedArray,labelsB,conjB::Char,beta::Number,C::StridedArray,labelsC;method::Symbol=:BLAS,buffer::TCBuffer=defaultcontractbuffer)
    # Updates C as beta*C+alpha*contract(A,B), whereby the contraction pattern
    # is specified by labelsA, labelsB and labelsC. The iterables labelsA(B,C)
    # should contain a unique label for every index of array A(B,C), such that
    # common labels of A and B correspond to indices that will be contracted.
    # Common labels between A and C or B and C indicate the position of the
    # uncontracted indices of A and B with respect to the indices of C, such
    # that the output array of the contraction can be added to C. Every label
    # should thus appear exactly twice in the union of labelsA, labelsB and
    # labelsC and the associated indices of the tensors should have identical
    # size.
    # Array A and/or B can be also conjugated by setting conjA and/or conjB
    # equal  to 'C' instead of 'N'.
    # The parametric argument method can be specified to choose between two
    # different contraction strategies:
    # -> method=:BLAS : permutes tensors (requires extra memory) and then
    #                   calls built-in (typically BLAS) multiplication
    # -> method=:native : memory-free native julia tensor contraction
    # -> method=:buffered : uses memory buffer (not implemented yet)

    # Get properties of input arrays
    NA=ndims(A)
    NB=ndims(B)
    NC=ndims(C)

    # Process labels, do some error checking and analyse problem structure
    if NA!=length(labelsA) || NB!=length(labelsB) || NC!=length(labelsC)
        throw(LabelError("invalid label specification"))
    end
    ulabelsA=unique(labelsA)
    ulabelsB=unique(labelsB)
    ulabelsC=unique(labelsC)
    if NA!=length(ulabelsA) || NB!=length(ulabelsB) || NC!=length(ulabelsC)
        throw(LabelError("tensorcontract requires unique label for every index of the tensor, handle inner contraction first with tensortrace"))
    end

    clabels=intersect(ulabelsA,ulabelsB)
    numcontract=length(clabels)
    olabelsA=intersect(ulabelsC,ulabelsA)
    numopenA=length(olabelsA)
    olabelsB=intersect(ulabelsC,ulabelsB)
    numopenB=length(olabelsB)

    if numcontract+numopenA!=NA || numcontract+numopenB!=NB || numopenA+numopenB!=NC
        throw(LabelError("invalid contraction pattern"))
    end

    # Compute and contraction indices and check size compatibility
    cindA=indexin(clabels,ulabelsA)
    oindA=indexin(olabelsA,ulabelsA)
    oindCA=indexin(olabelsA,ulabelsC)
    cindB=indexin(clabels,ulabelsB)
    oindB=indexin(olabelsB,ulabelsB)
    oindCB=indexin(olabelsB,ulabelsC)

    dimA=size(A)
    dimB=size(B)
    dimC=size(C)

    cdimsA=dimA[cindA]
    cdimsB=dimB[cindB]
    odimsA=dimA[oindA]
    odimsB=dimB[oindB]

    for i=1:numcontract
        cdimsA[i]==cdimsB[i] || throw(DimensionMismatch("dimension mismatch for label $(clabels[i])"))
    end
    for i=1:numopenA
        odimsA[i]==dimC[oindCA[i]] || throw(DimensionMismatch("dimension mismatch for label $(olabelsA[i])"))
    end
    for i=1:numopenB
        odimsB[i]==dimC[oindCB[i]] || throw(DimensionMismatch("dimension mismatch for label $(olabelsB[i])"))
    end

    # Perform contraction
    method==:BLAS && return tensorcontract_blas!(alpha,A,conjA,B,conjB,beta,C,buffer,oindA,cindA,oindB,cindB,oindCA,oindCB)
    method==:native && return NA>=NB ?
        tensorcontract_native!(alpha,A,conjA,B,conjB,beta,C,oindA,cindA,oindB,cindB,oindCA,oindCB) :
        tensorcontract_native!(alpha,B,conjB,A,conjA,beta,C,oindB,cindB,oindA,cindA,oindCB,oindCA)

    throw(ArgumentError("unknown contraction method"))
end

# Implementations
#-----------------
function tensorcontract_blas!(alpha::Number,A::StridedArray,conjA::Char,B::StridedArray,conjB::Char,beta::Number,C::StridedArray,buffer::TCBuffer,oindA,cindA,oindB,cindB,oindCA,oindCB)
    # The :BLAS method specification permutes A and B such that indopen and
    # indcontract are grouped, reshape them to matrices with all indopen on one
    # side and all indcontract on the other. Compute the data for C from
    # multiplying these matrices. Permute again to bring indices in requested
    # order.

    NA=ndims(A)
    NB=ndims(B)
    NC=ndims(C)
    TA=eltype(A)
    TB=eltype(B)
    TC=eltype(C)

    # only basic checking, this function is not expected to be called directly
    length(oindA)==length(oindCA) || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindB)==length(oindCB) || throw(DimensionMismatch("invalid contraction pattern"))
    length(cindA)==length(cindB)==div(NA+NB-NC,2) || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindA)+length(cindA)==NA || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindB)+length(cindB)==NB || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindCA)+length(oindCB)==NC || throw(DimensionMismatch("invalid contraction pattern"))

    # try to avoid extra allocation as much as possible
    if NC>0 && vcat(oindCB,oindCA)==collect(1:NC) # better to change role of A and B
        oindA,oindB=oindB,oindA
        cindA,cindB=cindB,cindA
        oindCA,oindCB=oindCB,oindCA
        A,B=B,A
        NA,NB=NB,NA
        TA,TB=TB,TA
    end

    dimsA=size(A)
    odimsA=dimsA[oindA]
    dimsB=size(B)
    odimsB=dimsB[oindB]
    cdims=dimsA[cindA]

    olengthA=prod(odimsA)
    olengthB=prod(odimsB)
    clength=prod(cdims)

    elsize=isbits(TC) ? sizeof(TC) : sizeof(Ptr)
    # permute A
    if conjA=='C'
        pA=vcat(cindA,oindA)
        if pA==collect(1:NA) && TA==TC && isa(A,Array)
            Amat=reshape(A,(clength,olengthA))
        else
            resize!(buffer.Abuf,length(A)*elsize)
            Amat=pointer_to_array(convert(Ptr{TC},pointer(buffer.Abuf)),tuple(cdims...,odimsA...))
            tensorcopy_native!(A,Amat,pA)
            Amat=reshape(Amat,(clength,olengthA))
        end
    elseif conjA=='N'
        if vcat(oindA,cindA)==collect(1:NA) && TA==TC && isa(A,Array)
            Amat=reshape(A,(olengthA,clength))
        elseif vcat(cindA,oindA)==collect(1:NA) && TA==TC && isa(A,Array)
            conjA='T'
            Amat=reshape(A,(clength,olengthA))
        else
            pA=vcat(cindA,oindA)
            conjA='T' # it is more efficient to compute At*B
            resize!(buffer.Abuf,length(A)*elsize)
            Amat=pointer_to_array(convert(Ptr{TC},pointer(buffer.Abuf)),tuple(cdims...,odimsA...))
            tensorcopy_native!(A,Amat,pA)
            Amat=reshape(Amat,(clength,olengthA))
        end
    else
        throw(ArgumentError("Value of conjA should be 'N' or 'C'"))
    end

    # permute B
    if conjB=='C'
        pB=vcat(oindB,cindB)
        if pB==collect(1:NB) && TB==TC && isa(B,Array)
            Bmat=reshape(B,(olengthB,clength))
        else
            resize!(buffer.Bbuf,length(B)*elsize)
            Bmat=pointer_to_array(convert(Ptr{TC},pointer(buffer.Bbuf)),tuple(odimsB...,cdims...))
            tensorcopy_native!(B,Bmat,pB)
            Bmat=reshape(Bmat,(olengthB,clength))
        end
    elseif conjB=='N'
        if vcat(cindB,oindB)==collect(1:NB) && TB==TC && isa(B,Array)
            Bmat=reshape(B,(clength,olengthB))
        elseif vcat(oindB,cindB)==collect(1:NB) && TB==TC && isa(B,Array)
            conjB='T'
            Bmat=reshape(B,(olengthB,clength))
        else
            pB=vcat(cindB,oindB)
            resize!(buffer.Bbuf,length(B)*elsize)
            Bmat=pointer_to_array(convert(Ptr{TC},pointer(buffer.Bbuf)),tuple(cdims...,odimsB...))
            tensorcopy_native!(B,Bmat,pB)
            Bmat=reshape(Bmat,(clength,olengthB))
        end
    else
        throw(ArgumentError("Value of conjA should be 'N' or 'C'"))
    end

    # calculate C
    pC=vcat(oindCA,oindCB)
    if pC==collect(1:NC) && isa(C,Array)
        Cmat=reshape(C,(olengthA,olengthB))
        Base.LinAlg.BLAS.gemm!(conjA,conjB,convert(TC,alpha),Amat,Bmat,convert(TC,beta),Cmat)
    else
        resize!(buffer.Cbuf,length(C)*elsize)
        Cmat=pointer_to_array(convert(Ptr{TC},pointer(buffer.Cbuf)),tuple(olengthA,olengthB))
        Base.LinAlg.BLAS.gemm!(conjA,conjB,one(TC),Amat,Bmat,zero(TC),Cmat)
        Cmat=reshape(Cmat,tuple(odimsA...,odimsB...))
        tensoradd_native!(alpha,Cmat,beta,C,invperm(pC))
    end
    return C
end

const CONTRACTGENERATE=[(1,1,2), # outer product of 2 vectors
(1,1,0), # scalar product of 2 vectors
(2,1,1), # mat vec multiplication
(1,2,1), # vec mat multiplication
(2,2,4), # mat mat outer product
(2,2,2), # mat mat multiplication
(2,2,0), # mat mat scalar product
(3,1,4),
(3,1,2),
(3,2,5),
(3,2,3),
(3,2,1),
(3,3,6),
(3,3,4),
(3,3,2),
(3,3,0),
(4,1,5),
(4,1,3),
(4,2,6),
(4,2,4),
(4,2,2),
(4,3,5), # restrict to outputs with NC<=6
(4,3,3),
(4,3,1),
(4,4,6),
(4,4,4),
(4,4,2),
(4,4,0)]

@eval @ngenerate (NA,NB,NC) typeof(C) $CONTRACTGENERATE function tensorcontract_native!{TA,NA,TB,NB,TC,NC}(alpha::Number,A::StridedArray{TA,NA},conjA::Char,B::StridedArray{TB,NB},conjB::Char,beta::Number,C::StridedArray{TC,NC},oindA,cindA,oindB,cindB,oindCA,oindCB)
    # only basic checking, this function is not expected to be called directly
    length(oindA)==length(oindCA) || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindB)==length(oindCB) || throw(DimensionMismatch("invalid contraction pattern"))
    length(cindA)==length(cindB)==div(NA+NB-NC,2) || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindA)+length(cindA)==NA || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindB)+length(cindB)==NB || throw(DimensionMismatch("invalid contraction pattern"))
    length(oindCA)+length(oindCB)==NC || throw(DimensionMismatch("invalid contraction pattern"))

    ostridesA=Int[stride(A,i) for i in oindA]
    cstridesA=Int[stride(A,i) for i in cindA]
    ostridesB=Int[stride(B,i) for i in oindB]
    cstridesB=Int[stride(B,i) for i in cindB]
    ostridesCA=Int[stride(C,i) for i in oindCA]
    ostridesCB=Int[stride(C,i) for i in oindCB]

    # calculate optimal contraction order for inner loops
    order=0
    if div(NA+NB-NC,2)==0 || ( NA-div(NA+NB-NC,2)>0 && minimum(ostridesA)<minimum(cstridesA) )
        order+=1 # k after i
    end
    if div(NA+NB-NC,2)==0 || ( NB-div(NA+NB-NC,2)>0 && minimum(ostridesB)<minimum(cstridesB) )
        order+=2 # k after j
    end
    if NA-div(NA+NB-NC,2)==0 || (NB-div(NA+NB-NC,2)>0 && minimum(ostridesB)<minimum(ostridesA))
        order+=4 # i after j
    end
    # more to left = later
    # j,i,k: order==0
    # j,k,i: order==1 || order==6
    # i,j,k: order==4
    # i,k,j: order==5 || order==2
    # k,j,i: order==3
    # k,i,j: order==7
    # 2 and 6 are the frustrated cases where no optimal order exists

    # calculate dims as variables
    olengthA=1
    olengthB=1
    clength=1
    @nexprs NA-div(NA+NB-NC,2) d->(odimsA_{d}=size(A,oindA[d]);olengthA*=odimsA_{d})
    @nexprs NB-div(NA+NB-NC,2) d->(odimsB_{d}=size(B,oindB[d]);olengthB*=odimsB_{d})
    @nexprs div(NA+NB-NC,2) d->(cdims_{d}=size(A,cindA[d]);clength*=cdims_{d})

    # calculate strides as variables
    @nexprs NA-div(NA+NB-NC,2) d->(begin
        ostridesA_{d}=ostridesA[d]
        ostridesCA_{d}=ostridesCA[d]
        minostridesA_{d}=min(ostridesA_{d},ostridesCA_{d})
    end)
    @nexprs NB-div(NA+NB-NC,2) d->(begin
        ostridesB_{d}=ostridesB[d]
        ostridesCB_{d}=ostridesCB[d]
        minostridesB_{d}=min(ostridesB_{d},ostridesCB_{d})
    end)
    @nexprs div(NA+NB-NC,2) d->(begin
        cstridesA_{d}=cstridesA[d]
        cstridesB_{d}=cstridesB[d]
        mincstrides_{d}=min(cstridesA_{d},cstridesB_{d})
    end)

    if beta==0
        fill!(C,zero(TC))
    end

    startA=1
    local Alinear::Array{TA,NA}
    if isa(A, SubArray)
        startA = A.first_index
        Alinear = A.parent
    else
        Alinear = A
    end
    startB=1
    local Blinear::Array{TB,NB}
    if isa(B, SubArray)
        startB = B.first_index
        Blinear = B.parent
    else
        Blinear = B
    end
    startC=1
    local Clinear::Array{TC,NC}
    if isa(C, SubArray)
        startC = C.first_index
        Clinear = C.parent
    else
        Clinear = C
    end

    if olengthA<=2*OBASELENGTH && olengthB<=2*OBASELENGTH && clength<=2*CBASELENGTH
        @gencontractkernel(NA-div(NA+NB-NC,2),NB-div(NA+NB-NC,2),div(NA+NB-NC,2),order,alpha,Alinear,conjA,Blinear,conjB,beta,Clinear,startA,startB,startC,odimsA,odimsB,cdims,ostridesA,cstridesA,ostridesB,cstridesB,ostridesCA,ostridesCB)
    else
        # build recursive stack
        depth=max(0,ceil(Integer, log2(olengthA/OBASELENGTH))+ceil(Integer, log2(olengthB/OBASELENGTH))+ceil(Integer, log2(clength/CBASELENGTH)))+4 # 4 levels safety margin
        level=1 # level of recursion
        stackpos=zeros(Int,depth) # record position of algorithm at the different recursion level
        stackpos[level]=0
        stackoblengthA=zeros(Int,depth)
        stackoblengthA[level]=olengthA
        stackoblengthB=zeros(Int,depth)
        stackoblengthB[level]=olengthB
        stackcblength=zeros(Int,depth)
        stackcblength[level]=clength
        @nexprs NA-div(NA+NB-NC,2) d->begin
            stackobdimsA_{d} = zeros(Int,depth)
            stackobdimsA_{d}[level] = odimsA_{d}
        end
        @nexprs NB-div(NA+NB-NC,2) d->begin
            stackobdimsB_{d} = zeros(Int,depth)
            stackobdimsB_{d}[level] = odimsB_{d}
        end
        @nexprs div(NA+NB-NC,2) d->begin
            stackcbdims_{d} = zeros(Int,depth)
            stackcbdims_{d}[level] = cdims_{d}
        end
        stackbstartA=zeros(Int,depth)
        stackbstartA[level]=startA
        stackbstartB=zeros(Int,depth)
        stackbstartB[level]=startB
        stackbstartC=zeros(Int,depth)
        stackbstartC[level]=startC
        stackgamma=zeros(typeof(beta),depth)
        stackgamma[level]=beta

        stackdA=zeros(Int,depth)
        stackdB=zeros(Int,depth)
        stackdC=zeros(Int,depth)
        stackdmax=zeros(Int,depth)
        stackwhichd=zeros(Int,depth)
        stacknewdim=zeros(Int,depth)
        stackolddim=zeros(Int,depth)

        while level>0
            pos=stackpos[level]
            oblengthA=stackoblengthA[level]
            oblengthB=stackoblengthB[level]
            cblength=stackcblength[level]
            @nexprs NA-div(NA+NB-NC,2) d->(obdimsA_{d} = stackobdimsA_{d}[level])
            @nexprs NB-div(NA+NB-NC,2) d->(obdimsB_{d} = stackobdimsB_{d}[level])
            @nexprs div(NA+NB-NC,2) d->(cbdims_{d} = stackcbdims_{d}[level])
            bstartA=stackbstartA[level]
            bstartB=stackbstartB[level]
            bstartC=stackbstartC[level]
            gamma=stackgamma[level]

            if (oblengthA<=OBASELENGTH && oblengthB<=OBASELENGTH && cblength<=CBASELENGTH) || level==depth # base case
                @gencontractkernel(NA-div(NA+NB-NC,2),NB-div(NA+NB-NC,2),div(NA+NB-NC,2),order,alpha,Alinear,conjA,Blinear,conjB,gamma,Clinear,bstartA,bstartB,bstartC,obdimsA,obdimsB,cbdims,ostridesA,cstridesA,ostridesB,cstridesB,ostridesCA,ostridesCB)
                level-=1
            elseif pos==0
                # find which dimension to divide
                dmax=0
                whichd=0
                maxval=0
                newdim=0
                olddim=0
                dA=0
                dB=0
                dC=0
                if oblengthA>=oblengthB && oblengthA*CBASELENGTH>=cblength*OBASELENGTH
                    whichd=1
                    @nexprs NA-div(NA+NB-NC,2) d->begin
                        newmax=obdimsA_{d}*minostridesA_{d}
                        if obdimsA_{d}>1 && newmax>maxval
                            dmax=d
                            olddim=obdimsA_{d}
                            newdim=olddim>>1
                            dA=ostridesA_{d}
                            dB=0
                            dC=ostridesCA_{d}
                            maxval=newmax
                        end
                    end
                elseif oblengthB>=oblengthA && oblengthB*CBASELENGTH>=cblength*OBASELENGTH
                    whichd=2
                    @nexprs NB-div(NA+NB-NC,2) d->begin
                        newmax=obdimsB_{d}*minostridesB_{d}
                        if obdimsB_{d}>1 && newmax>maxval
                            dmax=d
                            olddim=obdimsB_{d}
                            newdim=olddim>>1
                            dA=0
                            dB=ostridesB_{d}
                            dC=ostridesCB_{d}
                            maxval=newmax
                        end
                    end
                else
                    whichd=3
                    @nexprs div(NA+NB-NC,2) d->begin
                        newmax=cbdims_{d}*mincstrides_{d}
                        if cbdims_{d}>1 && newmax>maxval
                            dmax=d
                            olddim=cbdims_{d}
                            newdim=olddim>>1
                            dA=cstridesA_{d}
                            dB=cstridesB_{d}
                            dC=0
                            maxval=newmax
                        end
                    end
                end
                stackolddim[level]=olddim
                stacknewdim[level]=newdim
                stackdmax[level]=dmax
                stackwhichd[level]=whichd
                stackdA[level]=dA
                stackdB[level]=dB
                stackdC[level]=dC

                stackpos[level+1]=0
                stackoblengthA[level+1]= (whichd==1 ? div(oblengthA,olddim)*newdim : oblengthA)
                stackoblengthB[level+1]= (whichd==2 ? div(oblengthB,olddim)*newdim : oblengthB)
                stackcblength[level+1]= (whichd==3 ? div(cblength,olddim)*newdim : cblength)
                @nexprs NA-div(NA+NB-NC,2) d->(stackobdimsA_{d}[level+1] = (d==dmax && whichd==1 ? newdim : obdimsA_{d}))
                @nexprs NB-div(NA+NB-NC,2) d->(stackobdimsB_{d}[level+1] = (d==dmax && whichd==2 ? newdim : obdimsB_{d}))
                @nexprs div(NA+NB-NC,2) d->(stackcbdims_{d}[level+1] = (d==dmax && whichd==3 ? newdim : cbdims_{d}))
                stackbstartA[level+1]=bstartA
                stackbstartB[level+1]=bstartB
                stackbstartC[level+1]=bstartC
                stackgamma[level+1]=gamma

                stackpos[level]+=1
                level+=1
            elseif pos==1
                dmax=stackdmax[level]
                whichd=stackwhichd[level]
                olddim=stackolddim[level]
                newdim=stacknewdim[level]
                dA=stackdA[level]
                dB=stackdB[level]
                dC=stackdC[level]

                stackpos[level+1]=0
                stackoblengthA[level+1]= (whichd==1 ? div(oblengthA,olddim)*(olddim-newdim) : oblengthA)
                stackoblengthB[level+1]= (whichd==2 ? div(oblengthB,olddim)*(olddim-newdim) : oblengthB)
                stackcblength[level+1]= (whichd==3 ? div(cblength,olddim)*(olddim-newdim) : cblength)
                @nexprs NA-div(NA+NB-NC,2) d->(stackobdimsA_{d}[level+1] = (d==dmax && whichd==1 ? olddim-newdim : obdimsA_{d}))
                @nexprs NB-div(NA+NB-NC,2) d->(stackobdimsB_{d}[level+1] = (d==dmax && whichd==2 ? olddim-newdim : obdimsB_{d}))
                @nexprs div(NA+NB-NC,2) d->(stackcbdims_{d}[level+1] = (d==dmax && whichd==3 ? olddim-newdim : cbdims_{d}))
                stackbstartA[level+1]=bstartA+dA*newdim
                stackbstartB[level+1]=bstartB+dB*newdim
                stackbstartC[level+1]=bstartC+dC*newdim
                stackgamma[level+1]=(whichd==1 || whichd==2 ? gamma : one(gamma))

                stackpos[level]+=1
                level+=1
            else
                level-=1
            end
        end
    end
    return C
end


text:
struct CharEmbedding{M <: DenseMatrix}
   embedding :: M
end

@functor CharEmbedding

CharEmbedding(alphabetsize::Integer, embeddingdim=512) = CharEmbedding(Dense(alphabetsize, embeddingdim).W) |> gpu
function CharEmbedding(alphabet, embeddingdim=512)
   alphabetsize = length(alphabet)
   CharEmbedding(alphabetsize, embeddingdim) |> gpu
end

function Base.show(io::IO, m::CharEmbedding)
   embeddingdim, alphabetsize = size(m.embedding)
   print(io, "CharEmbedding($alphabetsize, $embeddingdim)")
end

(m::CharEmbedding)(charidxs::Integer) = m.embedding[:,charidxs]
function (m::CharEmbedding)(indices::DenseMatrix{<:Integer})
   time, batchsize = size(indices)
   embeddingdim = size(m.embedding, 1)
   output = permutedims(reshape(m.embedding[:,reshape(indices, Val(1))], embeddingdim, time, batchsize), (2,1,3))
   # #=check=# output == permutedims(cat(map(eachcol(indices)) do indicesᵢ
   #    m.embedding[:,indicesᵢ]
   # end...; dims=3), (2,1,3))
   return reshape(output, (time, 1, embeddingdim, batchsize)) # [t×c×b]
end

function convblock(nchannels::Pair{<:Integer,<:Integer} = (512=>512),
                   σ = identity,
                   filtersize::Integer = 5,
                   pdrop = 0.5f0;
                   pad = (filtersize-1)÷2, kwargs...)
   # "The convolutional layers in the network are regularized using dropout with probability 0.5"
   [Conv((filtersize,1), nchannels; pad=(pad,0), kwargs...),
    BatchNorm(last(nchannels), σ),
    Dropout(pdrop)]
end

###
"""
    BLSTM(in::Integer, out::Integer)

Constructs a bidirectional LSTM layer.
"""
struct BLSTM{M <: DenseMatrix, V <: DenseVector}
   forward  :: Recur{LSTMCell{M,V}}
   backward :: Recur{LSTMCell{M,V}}
   outdim   :: Int
end

Flux.trainable(m::BLSTM) = (m.forward, m.backward)
@functor BLSTM

function BLSTM(in::Integer, out::Integer)
   forward  = LSTM(in, out)
   backward = LSTM(in, out)
   return BLSTM(forward, backward, Int(out)) |> gpu
end

function Base.show(io::IO, m::BLSTM)
   in = size(m.forward.cell.Wi, 2)
   out = m.outdim
   print(io, "BLSTM($in, $out)")
end

"""
    (m::BLSTM)(Xs::DenseArray{<:Real,3}) -> DenseArray{<:Real,3}

Forward pass of the bidirectional LSTM layer for a 3D tensor input.
Input tensor must be arranged in T×D×B (time duration × input dimension × # batches) order.
"""
function (m::BLSTM)(Xs₄::DenseArray{<:Real,4})
   Xs = permutedims(dropdims(Xs₄; dims=2), (2, 3, 1)) # [t×d×b] -> [d×b×t]
   # preallocate output buffer
   Ys = Buffer(Xs, 2m.outdim, size(Xs,3), size(Xs,2))
   axisYs₁ = axes(Ys, 1)
   time_f = axes(Ys, 2)
   time_b = reverse(time_f)
   @inbounds begin
      # get forward and backward slice indices
      slice_f = axisYs₁[begin:m.outdim]
      slice_b = axisYs₁[(m.outdim+1):end]
      # bidirectional run step
      setindex!.((Ys,),  m.forward.(view.((Xs,), :, :, time_f)), (slice_f,), time_f, :)
      setindex!.((Ys,), m.backward.(view.((Xs,), :, :, time_b)), (slice_b,), time_b, :)
      # the same as
      # @views for (t_f, t_b) ∈ zip(time_f, time_b)
      #    Ys[slice_f, t_f, :] =  m.forward(Xs[:, :, t_f])
      #    Ys[slice_b, t_b, :] = m.backward(Xs[:, :, t_b])
      # end
      # but implemented via broadcasting as Zygote differentiates loops much slower than broadcasting
   end
   return copy(Ys) # [d×t×b]
end

# Flux.reset!(m::BLSTM) = reset!((m.forward, m.backward)) # not needed as taken care of by @functor

###
struct LocationAwareAttention{M₃ <: DenseMatrix, T <: DenseArray{<:Real,4}, M <: DenseMatrix, V′ <: DenseVector}
   dense :: Dense{typeof(identity), M, V′} # W & b
   F :: T
   U :: M
   V :: M
   w :: V′
end

@functor LocationAwareAttention

function LocationAwareAttention(dense::Dense{typeof(identity)}, F::T, U::M, V::M, w::V′) where {T <: DenseArray{<:Real,4}, M <: DenseMatrix, V′ <: DenseVector}
   M₃ = addparent(M, tensor₃of(M))
   LocationAwareAttention{M₃, T, M, V′}(dense, F, U, V, w)
end

function LocationAwareAttention(encodingdim=512, location_featuredim=32, attentiondim=128, querydim=1024, filtersize=31)
   @assert isodd(filtersize)
   dense = Dense(querydim, attentiondim)
   convF = Conv((filtersize,1), 2=>location_featuredim, pad = ((filtersize-1)÷2,0))
   denseU = Dense(location_featuredim, attentiondim)
   denseV = Dense(encodingdim, attentiondim)
   densew = Dense(attentiondim, 1)
   LocationAwareAttention(dense, convF.weight, denseU.W, denseV.W, vec(densew.W)) |> gpu
end

function Base.show(io::IO, m::LocationAwareAttention)
   encodingdim = size(m.V, 2)
   location_featuredim = size(m.F, 3)
   attentiondim, querydim = size(m.dense.W)
   print(io, "LocationAwareAttention($encodingdim, $location_featuredim, $attentiondim, $querydim)")
end

function (m::LocationAwareAttention)(values::T, keys::DenseArray{<:Real,3}, query::M, lastweights::M, Σweights::M) where {M <: DenseMatrix, T <: DenseArray{<:Real,3}}
   time, batchsize = size(lastweights)
   rdims = (time, 1, batchsize)
   lastweights³ = reshape(lastweights, rdims)
   Σweights³ = reshape(Σweights, rdims)
   weights_cat³ = [lastweights³ Σweights³]::T
   weights_cat⁴ = reshape(weights_cat³, (time, 1, 2, batchsize))
   pad = ((size(m.F, 1) - 1) ÷ 2, 0)
   cdims = DenseConvDims(weights_cat⁴, m.F; padding=pad)
   return m(values, keys, query, weights_cat⁴, cdims)
end

function (m::LocationAwareAttention{M₃})(values::DenseArray{<:Real,3}, keys::T₃₃, query::DenseMatrix, weights_cat⁴::DenseArray{<:Real,4}, cdims::DenseConvDims) where {M₃ <: DenseMatrix, T₃₃ <: DenseArray{<:Real,3}}
   attentiondim = length(m.w)
   _, time, batchsize = size(values)
   # location features
   fs = dropdims(conv(weights_cat⁴, m.F, cdims); dims=2) # F✶weights_cat
   Uf = reshape(m.U * reshape(permutedims(fs, (2,1,3)), Val(2)), (attentiondim, time, batchsize))
   # @ein Uf[a,t,b] := m.U[a,c] * fs[t,c,b] # dispatches to batched_contract (vetted)
   # Uf = einsum(EinCode{((1,2), (3,2,4)), (1,3,4)}(), (m.U, fs))::T₃₃
   # #=check=# Uf ≈ reduce(hcat, [reshape(m.U * fs[t,:,:], size(m.U,1), 1, :) for t ∈ axes(fs,1)])
   Ws⁺b = reshape(m.dense(query), (attentiondim, 1, batchsize)) # (a -> b) & (t -> c -> b)
   tanhWs⁺Vh⁺Uf⁺b = tanh.(Ws⁺b .+ keys .+ Uf)
   # @ein energies[t,b] := m.w[a] * tanhWs⁺Vh⁺Uf⁺b[a,t,b] # dispatches to batched_contract (vetted)
   energies = einsum(EinCode{((1,), (1,2,3)), (2,3)}(), (m.w, tanhWs⁺Vh⁺Uf⁺b))::M₃
   # #=check=# energies == reduce(vcat, [m.w'tanhWs⁺Vh⁺Uf⁺b[:,t,:] for t ∈ axes(tanhWs⁺Vh⁺Uf⁺b,2)])
   weights = softmax(energies) # α
   # @ein context[d,b] := values[d,t,b] * weights[t,b] # dispatches to batched_contract (vetted)
   context = einsum(EinCode{((1,2,3), (2,3)), (1,3)}(), (values, weights))::M₃
   # #=check=# context ≈ reduce(hcat, [sum(weights[t,b] * values[:,t,b] for t ∈ axes(values,2)) for b ∈ axes(values,3)])
   return context, weights
end

###
struct PreNet{D <: Dense}
   dense₁ :: D
   dense₂ :: D
   pdrop  :: Float32
end

Flux.trainable(m::PreNet) = (m.dense₁, m.dense₂)
@functor PreNet

function PreNet(in::Integer, out::Integer=256, pdrop=0.5f0, σ=leakyrelu)
   @assert 0 < pdrop < 1
   PreNet(Dense(in, out, σ), Dense(out, out, σ), Float32(pdrop)) |> gpu
end

function Base.show(io::IO, m::PreNet)
   out, in = size(m.dense₁.W)
   pdrop = m.pdrop
   σ = m.dense₁.σ
   print(io, "PreNet($in, $out, $pdrop, $σ)")
end

# "In order to introduce output variation at inference time, dropout with probability 0.5 is applied only to layers in the pre-net of the autoregressive decoder"
(m::PreNet)(x::DenseVecOrMat) = dropout(m.dense₂(dropout(m.dense₁(x), m.pdrop)), m.pdrop)

###
mutable struct State{M <: DenseMatrix}
   hs::NTuple{4, M}
end

Flux.trainable(m::State) = ()

@functor State

###
struct Decoder{M <: DenseMatrix, M₃ <: DenseMatrix, V <: DenseVector, T₄ <: DenseArray{<:Real,4}, D <: Dense}
   attention :: LocationAwareAttention{M₃, T₄, M, V}
   prenet    :: PreNet{D}
   lstms     :: Chain{NTuple{2, Recur{LSTMCell{M, V}}}}
   frameproj :: Dense{typeof(identity), M, V}
   state     :: State{M}
end

Flux.trainable(m::Decoder) = (m.attention, m.prenet, m.lstms, m.frameproj)
@functor Decoder

function Decoder(dims, filtersizes, pdrops)
   attention = LocationAwareAttention(dims.encoding, dims.location_feature, dims.attention, dims.query, filtersizes.attention)
   prenet = PreNet(dims.melfeatures, dims.prenet, pdrops.prenet)
   lstms = Chain(LSTM(dims.prenet + dims.encoding, dims.query), LSTM(dims.query, dims.query)) |> gpu
   frameproj = Dense(dims.query + dims.encoding, dims.melfeatures) |> gpu
   # initialize parameters
   query   = zeros(Float32, dims.query, 0)
   weights = zeros(Float32, 0, 0)
   frame   = zeros(Float32, dims.melfeatures, 0)
   state = State((query, weights, weights, frame)) |> gpu
   Decoder(attention, prenet, lstms, frameproj, state)
end

function Flux.reset!(m::Decoder)
   typeofdecoder = typeof(m)
   error("$typeofdecoder cannot be reset without information about input dimensions (batch size and its character length). Consider using reset!(m::Decoder, (time_in, batchsize)::NTuple{2,Integer}) or reset!(m::Decoder, input::DenseMatrix) instead.")
end

Flux.reset!(m::Decoder, input::DenseMatrix) = Flux.reset!(m, size(input))
function Flux.reset!(m::Decoder, (time_in, batchsize)::NTuple{2,Integer})
   Flux.reset!(m.lstms)
   #initialize dimensions
   querydim = size(m.attention.dense.W, 2)
   nmelfeatures = length(m.frameproj.b)
   # initialize parameters
   query   = gpu(zeros(Float32, querydim, batchsize))
   weights = gpu(zeros(Float32, time_in, batchsize))
   frame   = gpu(zeros(Float32, nmelfeatures, batchsize))
   m.state.hs = (query, weights, weights, frame)
   return nothing
end

function Base.show(io::IO, m::Decoder)
   print(io, """Decoder(
                   $(m.attention),
                   $(m.prenet),
                   $(m.lstms),
                   $(m.frameproj)
                )""")
end

function (m::Decoder{M})(values::DenseArray{<:Real,3}, keys::DenseArray{<:Real,3}) where M <: DenseMatrix
   (query, weights, Σweights, frame) = m.state.hs
   context, weights = m.attention(values, keys, query, weights, Σweights)
   Σweights += weights
   prenetoutput = m.prenet(frame)
   prenetoutput_context = [prenetoutput; context]::M
   query = m.lstms(prenetoutput_context)::M
   query_context = [query; context]::M
   frame = m.frameproj(query_context)
   m.state.hs = (query, weights, Σweights, frame)
   return frame, query_context
end

###
struct Tacotron₂{T₄ <: DenseArray{<:Real,4}, T₃₃ <: DenseArray{<:Real,3}, M <: DenseMatrix, M₃ <: DenseMatrix, V <: DenseVector, D <: Dense, C₃ <: Chain, C₅ <: Chain}
   che        :: CharEmbedding{M}
   convblock₃ :: C₃
   blstm      :: BLSTM{M, V}
   decoder    :: Decoder{M, M₃, V, T₄, D}
   stopproj   :: Dense{typeof(identity), M, V}
   postnet    :: C₅
end

@functor Tacotron₂

function Tacotron₂(che::CharEmbedding{M}, convblock₃::C₃, blstm::BLSTM{M,V}, decoder::Decoder{M,M₃,V,T₄,D}, stopproj::Dense{typeof(identity),M,V}, postnet::C₅) where {T₄ <: DenseArray{<:Real,4}, M <: DenseMatrix, M₃ <: DenseMatrix, V <: DenseVector, D <: Dense, C₃ <: Chain, C₅ <: Chain}
   T₃₃ = tensor₃of(M₃)
   Tacotron₂{T₄, T₃₃, M, M₃, V, D, C₃, C₅}(che, convblock₃, blstm, decoder, stopproj, postnet)
end

function Tacotron₂(dims::NamedTuple{(:alphabet,:encoding,:attention,:location_feature,:prenet,:query,:melfeatures,:postnet), <:NTuple{8,Integer}},
   filtersizes::NamedTuple{(:encoding,:attention,:postnet),<:NTuple{3,Integer}},
   pdrops::NamedTuple{(:encoding,:prenet,:postnet),<:NTuple{3,AbstractFloat}})
   @assert iseven(dims.encoding)

   che = CharEmbedding(dims.alphabet, dims.encoding)
   convblock₃ = Chain(reduce(vcat, map(_ -> convblock(dims.encoding=>dims.encoding, leakyrelu, filtersizes.encoding, pdrops.encoding), 1:3))...) |> gpu
   blstm = BLSTM(dims.encoding, dims.encoding÷2)

   decoder = Decoder(dims, filtersizes, pdrops)
   # will apply sigmoid implicitly during loss calculation with logitbinarycrossentropy for numerical stability
   stopproj = Dense(dims.query + dims.encoding, 1#=, σ=#) |> gpu

   nchmel, nch, fs, pdrop = dims.melfeatures, dims.postnet, filtersizes.postnet, pdrops.postnet
   postnet = Chain([convblock(nchmel=>nch, tanh, fs, pdrop);
                    convblock(nch=>nch, tanh, fs, pdrop);
                    convblock(nch=>nch, tanh, fs, pdrop);
                    convblock(nch=>nch, tanh, fs, pdrop);
                    convblock(nch=>nchmel, identity, fs, pdrop)]...) |> gpu
   Tacotron₂(che, convblock₃, blstm, decoder, stopproj, postnet)
end

function Tacotron₂(alphabet,
   otherdims=(encoding=512, attention=128, location_feature=32, prenet=256, query=1024, melfeatures=80, postnet=512),
   filtersizes = (encoding=5, attention=31, postnet=5),
   pdrop=0.5f0)
   dims = merge((alphabet = length(alphabet),), otherdims)
   pdrops = (encoding=pdrop, prenet=pdrop, postnet=pdrop)
   Tacotron₂(dims, filtersizes, pdrops)
end

function Flux.reset!(m::Tacotron₂)
   error("Tacotron₂ cannot be reset without information about input dimensions (batch size and its character length). Consider using reset!(m::Tacotron₂, time_in′batchsize::NTuple{2,Integer}) or reset!(m::Tacotron₂, input::DenseMatrix) instead.")
end

Flux.reset!(m::Tacotron₂, input::DenseMatrix) = Flux.reset!(m::Tacotron₂, size(input))
function Flux.reset!(m::Tacotron₂, time_in′batchsize::NTuple{2,Integer})
   Flux.reset!(m.blstm)
   Flux.reset!(m.decoder, time_in′batchsize)
end

function Base.show(io::IO, m::Tacotron₂)
   print(io, """Tacotron₂(
                   $(m.che),
                   $(m.convblock₃),
                   $(m.blstm),
                   $(m.decoder),
                   $(m.stopproj),
                   $(m.postnet)
                )""")
end

function (m::Tacotron₂{T₄,T₃₃})(textindices::DenseMatrix{<:Integer}, time_out::Integer) where {T₄ <: DenseArray{<:Real,4}, T₃₃ <: DenseArray{<:Real,3}}
   # dimensions
   batchsize = size(textindices, 2)
   nmelfeatures = length(m.decoder.frameproj.b)
   # encoding stage
   chex = m.che(textindices)
   convblock₃x = m.convblock₃(chex)::T₄
   values = m.blstm(convblock₃x)
   # @ein keys[a,t,b] := m.attention.V[a,d] * values[d,t,b] # dispatches to batched_contract (vetted)
   keys = einsum(EinCode{((1,2), (2,3,4)), (1,3,4)}(), (m.decoder.attention.V, values))::T₃₃
   # #=check=# Vh ≈ reduce(hcat, [reshape(m.decoder.attention.V * values[:,t,:], size(m.V,1), 1, :) for t ∈ axes(values,2)])
   decodings = (_ -> m.decoder(values, keys)).(1:time_out)
   frames = first.(decodings)
   query_contexts = last.(decodings)
   σ⁻¹stoprobs = reshape(m.stopproj(reduce(hcat, query_contexts)), (batchsize, time_out))
   # #=check=# σ⁻¹stoprobs == reduce(hcat, reshape.(m.stopproj.(query_contexts), Val(1)))
   prediction = reshape(reduce(hcat, frames), (nmelfeatures, batchsize, time_out))
   # #=check=# prediction == cat(frames...; dims=3)
   melprediction = reshape(permutedims(prediction, (3,1,2)), (time_out, 1, nmelfeatures, batchsize))
   melprediction⁺residual = melprediction + m.postnet(melprediction)::T₄
   return melprediction, melprediction⁺residual, σ⁻¹stoprobs
end

###
function loss(model::Tacotron₂, textindices::DenseMatrix{<:Integer}, meltarget::DenseArray{<:Real,4}, stoptarget::DenseMatrix{<:Real})
   melprediction, melprediction⁺residual, σ⁻¹stoprobs = model(textindices, size(meltarget, 1))
   l = mse(melprediction, meltarget) +
       mse(melprediction⁺residual, meltarget) +
       mean(logitbinarycrossentropy.(σ⁻¹stoprobs, stoptarget))
   return l
end

loss(model::Tacotron₂, (textindices, meltarget, stoptarget)::Tuple{DenseMatrix{<:Integer}, DenseArray{<:Real,4}, DenseMatrix{<:Real}}) = loss(model, textindices, meltarget, stoptarget)

ext: jl


text:
using Test
using MDTrajAnalyse

@testset "Cell" begin
    c = CubicCell(1.1)
    a = parsecell(1.1)
    aa = parsecell(1.1,1.1,1.1)
    naa = parsecell(1.2,1.1,1.1)
    @test c == a
    @test c == aa
    @test typeof(aa) != typeof(naa)

    v = VariableCell([CubicCell(2.3), CubicCell(3.4), OrthorombicCell(2.3,2.4,2.5)] )
    @test length(v) == 3
    @test lastindex(v) == 3

    q = iterate(v)

    @test q[1] == v[1]

    @test length(volume(v)) == 3
end

@testset "Trajectorys" begin
    t = Trajectory(rand(3,4,5))
    tt = Trajectory(t.xyz; cell=CubicCell(1.0))
    @test view(t,1) == view(tt,1)
    distances(t, 1,2)
    compute_rdf(tt, 1:1, 2:3)
end


text:
# Note that this script can accept some limited command-line arguments, run
# `julia build_tarballs.jl --help` to see a usage message.
using BinaryBuilder, Pkg

name = "COSMA"
version = v"2.2.0"

# Collection of sources required to complete build
sources = [
    ArchiveSource("https://github.com/eth-cscs/COSMA/releases/download/v2.2.0/cosma.tar.gz", "1eb92a98110df595070a12193b9221eecf9d103ced8836c960f6c79a2bd553ca")
]

# Bash recipe for building across all platforms
script = raw"""
cd $WORKSPACE/srcdir

if [[ "$nbits" == "64" ]] && [[ "$target" != aarch64-* ]]; then
    BLAS_LAPACK_LIB="$libdir/libopenblas64_.$dlext"

    # Fix suffixes for 64-bit OpenBLAS
    SYMB_DEFS=()
    for sym in cblas_sgemm cblas_dgemm cblas_cgemm cblas_zgemm; do
        SYMB_DEFS+=("-D${sym}=${sym}64_")
    done
    export CXXFLAGS="${SYMB_DEFS[@]}"
else
    BLAS_LAPACK_LIB="$libdir/libopenblas.$dlext"
fi

if [[ "$target" == x86_64-apple-darwin14 ]] || [[ "$target" == x86_64-unknown-freebsd11.1 ]]; then
    OMP_LIB=`find $libdir -iname 'libgomp*'`
    OPENMP_CMAKE_FLAGS="-DOpenMP_CXX_FLAGS=-fopenmp=libgomp -DOpenMP_CXX_LIB_NAMES=gomp -DOpenMP_gomp_LIBRARY=$OMP_LIB"
    OMP_HEADER=`find / -name omp.h 2>/dev/null  | head -n1`
    mkdir include
    cp $OMP_HEADER include/
    export CPATH=$PWD/include
else
    OPENMP_CMAKE_FLAGS=
fi

mkdir build
cd build

cmake ../cosma \
    -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TARGET_TOOLCHAIN} \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_INSTALL_PREFIX=$prefix \
    -DCOSMA_WITH_TESTS=OFF \
    -DCOSMA_WITH_APPS=OFF \
    -DCOSMA_WITH_BENCHMARKS=OFF \
    -DCOSMA_WITH_INSTALL=ON \
    -DCOSMA_WITH_PROFILING=OFF \
    -DBUILD_SHARED_LIBS=ON \
    -DCOSMA_BLAS=OPENBLAS \
    -DCOSMA_SCALAPACK=OFF \
    -DMPI_CXX_COMPILER=$bindir/mpicxx \
    -DMPI_C_COMPILER=$bindir/mpicc \
    -DOPENBLAS_LIBRARIES=$BLAS_LAPACK_LIB \
    -DOPENBLAS_INCLUDE_DIR=$includedir \
    $OPENMP_CMAKE_FLAGS

make -j$(nproc)
make install

install_license ../cosma/LICENCE
"""

# These are the platforms we will build for by default, unless further
# platforms are passed in on the command line
platforms = supported_platforms()
platforms = expand_cxxstring_abis(platforms)
filter!(p -> !(p isa Windows), platforms)

# The products that we will ensure are always built
products = [
    LibraryProduct("libgrid2grid", :grid2grid),
    LibraryProduct("libcosma", :cosma)
]

# Dependencies that must be installed before this package can be built
dependencies = [
    Dependency(PackageSpec(name="OpenBLAS_jll", uuid="4536629a-c528-5b80-bd46-f80d51c5b363")),
    Dependency(PackageSpec(name="MPICH_jll", uuid="7cb0a576-ebde-5e09-9194-50597f1243b4")),
    Dependency(PackageSpec(name="CompilerSupportLibraries_jll", uuid="e66e0078-7015-5450-92f7-15fbd957f2ae"))
]

# Build the tarballs, and possibly a `build.jl` as well.
build_tarballs(ARGS, name, version, sources, script, platforms, products, dependencies; preferred_gcc_version = v"7.1.0")


text:
using Test
using HarmonicPowerFlow
using DataFrames

# test against pypsa

@testset "admittance_matrices" begin
    nodes = DataFrame(
        ID = 1:5,
        type = ["slack", "PQ", "PQ", "PQ", "nonlinear"],
        component = ["generator", "lin_load_1", "lin_load_2", "lin_load_3", "smps"],
        S = [1000, 0, 0, 0, 0],
        P = [0, 100, 100, 0, 250],
        Q = [0, 100, 100, 0, 100],
        X_sh = [0.005, 0, 0, 0, 0])
    lines = DataFrame(
        ID = 1:5,
        fromID = 1:5,
        toID = [2,3,4,5,1],
        R = [0.5, 1, 0.5, 0.5, 0.5],
        X = [0.5, 4, 1, 1, 1],
        G = [0, 0, 0 ,0 ,0],
        B = [0, 0, 0 ,0 ,0])
    settings = HarmonicPowerFlow.init_settings(true, [1, 3, 5])
    net = HarmonicPowerFlow.init_power_grid(nodes, lines, settings)
    LY = HarmonicPowerFlow.admittance_matrices(net, [1, 3, 5])
    @test size(LY[1]) == (5, 5)
    @test length(LY) == 3
end


text:
using egads

# const libegads_include = "/home/mopg/code/EngSketchPad/lib/libegads.so"
# include the above in the precompilation step. use libdl.find_library to look for common places where this library file is located

egO   = Ref{ego}() # Pointer to a pointer essentially
egO_2 = Ref{ego}()
egO_3 = Ref{ego}()
egO_4 = Ref{ego}()
egO_5 = Ref{ego}()
egO_6 = Ref{ego}()
egO_O_1 = Ref{Ptr{ego}}()

# the other way to do this is in the function
# function blabla(cid)
#            ptrobj = Ref{ego}()
#            call c function bullshit
#            return ptrobj[]
# end

# if you want to initialize a pointer to an object, you need to use for example
#       bla = ego( 0 ) # initialize to something

width = 5.0
rad   = 1.0

status = EG_open(egO)
if (status < 0) error("Can't open, failure code: %i", status) end
context = egO[] # now dereference the pointer to obtain the
@show context

# build cylinder
datacyl = [0.0, 0.0,  width/2.0,
           0.0, 0.0, -width/2.0,
           rad]
status = EG_makeSolidBody(context, CYLINDER, datacyl, egO_2)
ebody1 = egO_2[] # dereference the pointer to obtain the pointer to egObject
if (status != EGADS_SUCCESS) cleanup(status, context) end

# build end points of sphere
datasph = [0.0, 0.0, -width/2.0, rad]
status = EG_makeSolidBody(context, SPHERE, datasph, egO_3)
ebody2 = egO_3[] # dereference the pointer to obtain the pointer to egObject
if (status != EGADS_SUCCESS) cleanup(status, context) end
datasph = [0.0, 0.0,  width/2.0, rad]
status = EG_makeSolidBody(context, SPHERE, datasph, egO_4)
ebody3 = egO_4[] # dereference the pointer to obtain the pointer to egObject
if (status != EGADS_SUCCESS) cleanup(status, context) end

# boolean operations
status = EG_solidBoolean(ebody1, ebody2, FUSION, egO_5)
emodel2 = egO_5[]
if (status != EGADS_SUCCESS) cleanup(status, context) end

# add second sphere
oclass0 = Ref{Int32}(0)
mtype0  = Ref{Int32}(0)
nchild0 = Ref{Int32}(0)
senses0 = Ref{Ptr{Int32}}(0)
data0   = fill(0.0, 4)
status  = EG_getTopology( emodel2 , egO_2 , oclass0 , mtype0 ,
                          data0   , nchild0 , egO_O_1 ,  senses0 )
if (status != EGADS_SUCCESS) cleanup(status, context) end
carray = egO_O_1[]
child0 = unsafe_load(carray,1)

status = EG_solidBoolean(child0, ebody3, FUSION, egO_6)
emodel = egO_6[]
if (status != EGADS_SUCCESS) cleanup(status, context) end

# save model
status = EG_saveModel(emodel, "test.egads")
if (status != EGADS_SUCCESS) cleanup(status, context) end
status = EG_saveModel(emodel, "test.step")
if (status != EGADS_SUCCESS) cleanup(status, context) end

# delete
status = EG_deleteObject(ebody1)
if (status != EGADS_SUCCESS) cleanup(status, context) end
status = EG_deleteObject(ebody2)
if (status != EGADS_SUCCESS) cleanup(status, context) end
status = EG_deleteObject(ebody3)
if (status != EGADS_SUCCESS) cleanup(status, context) end
status = EG_deleteObject(emodel)
if (status != EGADS_SUCCESS) cleanup(status, context) end
status = EG_deleteObject(emodel2)
if (status != EGADS_SUCCESS) cleanup(status, context) end

# close everything
cleanup( context )


text:
using LeastSquaresOptim

@test_broken begin
println("Use LeastSquaresOptim to fit the parameter")
cost_function = build_lsoptim_objective(prob1,t,data,Tsit5(),verbose=false)
x = [1.0]
res = LeastSquaresOptim.optimize!(LeastSquaresOptim.LeastSquaresProblem(x = x,
                f! = cost_function,
                output_length = length(t)*length(prob1.u0)),
                LeastSquaresOptim.Dogleg(LeastSquaresOptim.LSMR()))
@test result.minimizer[1] ≈ 1.5 atol=3e-1
cost_function = build_lsoptim_objective(prob2,t,data,Tsit5(),verbose=false)
x = [1.3,2.7]
res = LeastSquaresOptim.optimize!(LeastSquaresOptim.LeastSquaresProblem(x = x,
                f! = cost_function,
                output_length = length(t)*length(prob2.u0)),
                LeastSquaresOptim.Dogleg(LeastSquaresOptim.LSMR()))
@test res.minimizer ≈ [1.5;3.0] atol=3e-1
cost_function = build_lsoptim_objective(prob3,t,data,Tsit5(),verbose=false)
x = [1.3,0.8,2.8,1.2]
res = LeastSquaresOptim.optimize!(LeastSquaresOptim.LeastSquaresProblem(
                x = x, f! = cost_function,
                output_length = length(t)*length(prob3.u0)),
                LeastSquaresOptim.Dogleg(LeastSquaresOptim.LSMR()))
@test res.minimizer ≈ [1.5;1.0;3.0;1.0] atol=3e-1
end


text:
@testset "Habitat catchability" begin
    habq_spec = HabQSpec(1, 1.0)
    run_sims(HabQSpec, 1;
             base_dir = ".",
             csv = true,
             feather = true)

    # Construct all of the expected output file names
    output_files = joinpath.("habq",
                             "repl_01",
                             "habq_" .*
                             string.(1:length(sim_values(HabQSpec)),
                                     pad = 2) .*
                             ["_popstate.h5" "_popstate.csv" "_catch.csv" "_popstate.feather" "_catch.feather"])

    @test prep_file(habq_spec) == "habq/repl_01/habq_prep.h5"
    @test isdir("habq")
    @test isdir("habq", "repl_01")
    @test all(isfile.(output_files))
end


text:
using QuantumCumulants
using OrdinaryDiffEq
using ModelingToolkit
using Test

@testset "v-level" begin

# Hilbertspace
hf = FockSpace(:cavity)
ha = NLevelSpace(:atom, 3)
h = hf⊗ha

# Parameters
@cnumbers κ g
Δc, Γ2, Γ3, Δ2, Δ3, Ω2, Ω3 = cnumbers("Δ_c Γ_2 Γ_3 Δ_2 Δ_3 Ω_2 Ω_3")

# Operators
@qnumbers a::Destroy(h) σ::Transition(h)

# Hamiltonian
H_atom = -Δ2*σ(2,2) - Δ3*σ(3,3) + Ω2*(σ(2,1) + σ(1,2)) + Ω3*(σ(3,1) + σ(1,3))
H_cav = -Δc*a'*a + g*(a'*σ(1,2) + a*σ(2,1))
H = H_atom + H_cav

# Jump operators
J = [a,σ(1,2),σ(1,3)]
rates = [κ,Γ2,Γ3]
ops = [a'*a]

# Derive first equation
he_n = meanfield([a'*a],H,J;rates=rates)

# Average and expand to 2nd order
hn_avg = cumulant_expansion(he_n,2)

# Complete the system
he_comp = complete(hn_avg)

# Compare to finding operators from start
ops = find_operators(h,2)

@test length(ops)==length(he_comp)==16
he = meanfield(ops,H,J;rates=rates)
he_avg = cumulant_expansion(he,2)

@test all(((l ∈ Set(he_comp.states) || QuantumCumulants._adjoint(l) ∈ Set(he_comp.states))) for l in he_avg.states)
@test all(((l ∈ Set(he_comp.states) || QuantumCumulants._adjoint(l) ∈ Set(he_avg.states))) for l in he_comp.states)

p = [κ, g, Δc, Γ2, Γ3, Δ2, Δ3, Ω2, Ω3]
@named sys = ODESystem(he_avg)

u0 = zeros(ComplexF64,length(he_avg))

# Parameters - numerical values
Γ3n = 1.0
Γ2n = Γ3n/4266
κn = Γ3n/32.0
Δ2n = 0.5 * Γ3n
Δ3n = -0.2 .* Γ3n
Δcn = Δ2n
Ω2n = 0.2 * Γ3n
Ω3n = 0.05 * Γ3n
gn = 10.0 * Γ2n
tmax = 5/Γ2n

p0 = p .=> [κn, gn, Δcn, Γ2n, Γ3n, Δ2n, Δ3n, Ω2n, Ω3n]
prob = ODEProblem(sys,u0,(0.0,tmax),p0)
sol = solve(prob,RK4(),jac=true,sparse=true)

avg = average(a'*σ(2,1))
@test sol[avg] == map(conj, getindex.(sol.u, 7)) == map(conj, sol[QuantumCumulants._conj(avg)])

# Filter cavity equations to compute spectrum
# Hilbert space
hfilter = FockSpace(:filter)
h_tot = h⊗hfilter

# New cnumbers
ωf, κf, gf = cnumbers("ω_f κ_f g_f")

# Operators
c = Destroy(h_tot,:c, 3)
af = Destroy(h_tot,:a, 1)
σf(i,j) = Transition(h_tot, :σ, i, j)

# Hamiltonian and jumps
H_atom_f = -Δ2*σf(2,2) - Δ3*σf(3,3) + Ω2*(σf(2,1) + σf(1,2)) + Ω3*(σf(3,1) + σf(1,3))
H_cav_f = -Δc*af'*af + g*(af'*σf(1,2) + af*σf(2,1))
Hf = ωf*c'*c + gf*(af'*c + af*c') + H_atom_f + H_cav_f

Jf = [af,σf(1,2),σf(1,3),c]
rates_f = [rates;κf]

# Gather operators
ops_f = find_operators(h_tot,2;names=[:a,:σ,:c])
filter!(x->(3 in acts_on(x)), ops_f)

# Compute equations
he_f = meanfield(ops_f,Hf,Jf;rates=rates_f)
he_f_avg = cumulant_expansion(he_f,2)

# Find missing averages and them as parameter
missing_avgs = find_missing(he_f_avg)
avg_ps = QuantumCumulants._make_parameter.(missing_avgs)

he_f_avg = substitute(he_f_avg, Dict(missing_avgs .=> avg_ps))

# Gather all new cnumbers
pf = [ωf; gf; κf; avg_ps; p]

# Generate function for the filter cavities
@named sys_f = ODESystem(he_f_avg)

end # testset


text:
export EndgameTracker, EndgameOptions, track

"""
    AbstractPathTracker

Supertype for path trackers.
"""
abstract type AbstractPathTracker end

Base.broadcastable(T::AbstractPathTracker) = Ref(T)

"""
    EndgameOptions(; options...)

Options controlling the behaviour of a [`EndgameTracker`](@ref).

## Options

* `at_infinity_check = true`: Whether divering paths should be truncated.
* `endgame_start = 0.1`: The point `t` in time where the endgame starts.
* `only_nonsingular = false`: If `true` don't run the Cauchy endgame to handle singular
  solutions.
* `zero_is_at_infinity = false`: Whether paths going to a solution where at least one
  coordinates is zero should also be considered diverging.

### Parameters
These parameters control the behaviour during the endgame. See [^BT20] for details.

* `min_cond = 1e3`: The minimal growth of the condition number after which an
  endgame strategy is considered to be applied.
* `max_endgame_steps = 2000`: The maximal number of steps performed during the endgame.
* `max_winding_number = 12`: The maximal winding number which is attempted in the
 Cauchy endgame.
* `min_growth = 100`: The minimal relative growth of a coordinate necessary to
  to be considered going to infininity (resp. zero).
* `val_at_infinity_tol = 1e-3`: Tolerance on the valuation which has to be
  satisfied before a path is considered to diverge / go to infinity.
* `val_finite_tol = 1e-3`: Tolerance on the valuation which has to be satisfied
  before the Cauchy endgame is started.

[^BT20]: Breiding, P. and Timme, S. "Tropical Endgame", In preparation (2020)
"""
Base.@kwdef mutable struct EndgameOptions
    endgame_start::Float64 = 0.1
    max_endgame_steps::Int = 2000
    # eg parameter
    min_cond::Float64 = 1e4
    min_coord_growth::Float64 = 100.0
    zero_is_at_infinity::Bool = false
    at_infinity_check::Bool = true
    only_nonsingular::Bool = false
    # valuation etc
    val_finite_tol::Float64 = 1e-3
    val_at_infinity_tol::Float64 = 1e-3

    # singular solutions parameters
    max_winding_number::Int = 12
end

Base.show(io::IO, opts::EndgameOptions) = print_fieldnames(io, opts)
Base.show(io::IO, ::MIME"application/prs.juno.inline", opts::EndgameOptions) = opts

"""
    EndgameTrackerCode

The possible states an `EndgameTracker` can be in:

* `EndgameTrackerCode.tracking`
* `EndgameTrackerCode.success`
* `EndgameTrackerCode.at_infinity`
* `EndgameTrackerCode.at_zero`
* `EndgameTrackerCode.excess_solution`
* `EndgameTrackerCode.post_check_failed`
* `EndgameTrackerCode.polyhedral_failed`
* `EndgameTrackerCode.terminated_accuracy_limit`
* `EndgameTrackerCode.terminated_ill_conditioned`
* `EndgameTrackerCode.terminated_invalid_startvalue`
* `EndgameTrackerCode.terminated_max_winding_number`
* `EndgameTrackerCode.terminated_max_steps`
* `EndgameTrackerCode.terminated_step_size_too_small`
"""
module EndgameTrackerCode
using ..TrackerCode: TrackerCode

@enum codes begin
    tracking
    success
    at_infinity
    at_zero
    terminated_accuracy_limit
    terminated_invalid_startvalue
    terminated_ill_conditioned
    terminated_max_steps
    terminated_max_winding_number
    terminated_step_size_too_small
    terminated_unknown
    post_check_failed
    excess_solution
    polyhedral_failed
end

function Base.convert(::Type{codes}, code::TrackerCode.codes)
    if code == TrackerCode.success
        return success
    elseif code == TrackerCode.terminated_max_steps
        return terminated_max_steps
    elseif code == TrackerCode.terminated_accuracy_limit
        return terminated_accuracy_limit
    elseif code == TrackerCode.terminated_ill_conditioned
        return terminated_ill_conditioned
    elseif code == TrackerCode.terminated_invalid_startvalue
        return terminated_invalid_startvalue
    elseif code == TrackerCode.terminated_step_size_too_small
        return terminated_step_size_too_small
    elseif code == TrackerCode.terminated_unknown
        return terminated_unknown
    else
        return tracking
    end
end

end

"""
    is_success(code::EndgameTrackerCode.codes)

Returns `true` if `status` indicates a success in tracking.
"""
is_success(code::EndgameTrackerCode.codes) = code == EndgameTrackerCode.success

"""
    is_success(code::EndgameTrackerCode.codes)

Returns `true` if `status` indicates that a path diverged towards infinity.
"""
is_at_infinity(code::EndgameTrackerCode.codes) = code == EndgameTrackerCode.at_infinity

"""
    is_tracking(code::EndgameTrackerCode.codes)

Returns `true` if `status` indicates the tracking is not going on.
"""
is_tracking(code::EndgameTrackerCode.codes) = code == EndgameTrackerCode.tracking

"""
    is_invalid_startvalue(code::EndgameTrackerCode.codes)

Returns `true` if the provided start value was not valid.
"""
is_invalid_startvalue(code::EndgameTrackerCode.codes) =
    code == EndgameTrackerCode.terminated_invalid_startvalue

###
### State
###
Base.@kwdef mutable struct EndgameTrackerState
    code::EndgameTrackerCode.codes = EndgameTrackerCode.tracking
    val::Valuation
    endgame_started::Bool = false
    row_scaling::Vector{Float64}
    col_scaling::Vector{Float64}
    cond_eg_start::Float64 = 1.0
    steps_eg::Int = 0
    solution::Vector{ComplexF64}
    t_last_cauchy::Float64 = NaN
    winding_number::Union{Nothing,Int} = nothing
    accuracy::Float64 = NaN
    cond::Float64 = 1.0
    prediction::Vector{ComplexF64}
    last_point::Vector{ComplexF64}
    last_t::Float64 = NaN
    max_winding_number_hit::Bool = false
    cauchy_failures::Int = 0
    jump_to_zero_failed::Tuple{Bool,Bool} = (false, false)
end

EndgameTrackerState(npolynomials::Integer, x::AbstractVector) = EndgameTrackerState(;
    val = Valuation(length(x)),
    row_scaling = zeros(npolynomials),
    col_scaling = zeros(length(x)),
    solution = copy(x),
    prediction = copy(x),
    last_point = copy(x),
)

"""
    EndgameTracker(tracker::Tracker; options = EndgameOptions())
    EndgameTracker(H::AbstractHomotopy; options = EndgameOptions())

A `EndgameTracker` combines a [`Tracker`](@ref) with an endgame. That is,
while a [`Tracker`](@ref) assumes that the solution path is non-singular and convergent, the endgame
allows to handle singular endpoints as well as diverging paths.
To compute singular solutions the *Cauchy endgame* used, for divering paths a strategy
based on the valuation of local Puiseux series expansion of the path is used.
See [^BT20] for a detailed description.
By convention, a `EndgameTracker` always tracks from ``t=1`` to ``t = 0``.
See [`EndgameOptions`](@ref) for the possible options.

[^BT20]: Breiding, P. and Timme, S. "Tropical Endgame", In preparation (2020)
"""
struct EndgameTracker{
    H<:AbstractHomotopy,
    N, # AutomaticDifferentiation
    M<:AbstractMatrix{ComplexF64},
} <: AbstractPathTracker
    tracker::Tracker{H,N,M}
    state::EndgameTrackerState
    options::EndgameOptions
end

EndgameTracker(H::Homotopy; kwargs...) = EndgameTracker(ModelKitHomotopy(H); kwargs...)
function EndgameTracker(H::AbstractHomotopy; tracker_options = TrackerOptions(), kwargs...)
    EndgameTracker(Tracker(H; options = tracker_options); kwargs...)
end
function EndgameTracker(tracker::Tracker; options = EndgameOptions())
    if !(options isa EndgameOptions)
        options = EndgameOptions(; options...)
    end
    state = EndgameTrackerState(size(tracker.homotopy, 1), tracker.state.x)
    EndgameTracker(tracker, state, options)
end

Base.broadcastable(T::EndgameTracker) = Ref(T)

function init!(
    endgame_tracker::EndgameTracker,
    x,
    t₁::Real;
    ω::Float64 = NaN,
    μ::Float64 = NaN,
)
    @unpack tracker, state, options = endgame_tracker

    init!(tracker, x, t₁, 0.0; ω = ω, μ = μ)

    state.code = status(tracker)
    init!(state.val)
    state.endgame_started = false
    state.row_scaling .= 1
    state.col_scaling .= weights(tracker.state.norm)
    state.cond_eg_start = 1.0
    state.cond = 1.0
    state.solution .= NaN
    state.accuracy = NaN
    state.t_last_cauchy = NaN
    state.winding_number = nothing
    state.steps_eg = 0
    state.max_winding_number_hit = false
    state.cauchy_failures = 0
    state.jump_to_zero_failed = (false, false)

    endgame_tracker
end

"""
    CauchyEndgameResult

An enum indicating the result of the [`cauchy!`](@ref) computation.

# Cases
* `CAUCHY_SUCCESS`: The endgame was successfull.
* `CAUCHY_TERMINATED_MAX_WINDING_NUMBER`: The endgame was terminated since the winding
  number is larger than the provided threshold.
* `CAUCHY_TERMINATED`: The endgame was terminated due to some other error in the path
  tracking.
"""
@enum CauchyEndgameResult begin
    CAUCHY_SUCCESS
    CAUCHY_TERMINATED_MAX_WINDING_NUMBER
    CAUCHY_TERMINATED
end

"""
    cauchy!(state::EndgameTrackerState, tracker::Tracker, options::EndgameOptions)

Try to predict the value of `x(0)` using the [`CauchyEndgame`](@ref).
For this we track the polygon defined by ``te^{i2πk/n}`` until we end again at ``x``.
Here ``n`` is the number of samples we take per loop, `samples_per_loop`.
The computation gives up if we have a winding number larger than `max_winding_number`.
It returns a tuple denoting the success ([`CauchyEndgameResult`](@ref)) the computed
winding number `m::Int` and th expected accuracy of the solution.

[Cauchy's integral formula]: https://en.wikipedia.org/wiki/Cauchy%27s_integral_formula
"""
function cauchy!(state::EndgameTrackerState, tracker::Tracker, options::EndgameOptions)
    @unpack last_point, prediction = state

    t = real(tracker.state.t)
    # Mathemtically, we only need `n₀ = ceil(Int, log(eps()) / log(t))` many sample points
    # to achieve the maximal accuracy since the error is ≈ t^n₀.
    # However, we have to be careful that we are not getting too close to the singularity
    # during tracking. E.g. with `n₀ = 3` we track fairly close to the origin for the
    # first first and third path. So we require at least 8 sample points.
    n₀ = max(ceil(Int, log(eps()) / log(t)), 8)
    @unpack x, μ, ω = tracker.state

    # always use extended precision for cauchy endgame
    prediction_acc = use_extended_precision!(tracker)
    # fix tracker to not flip between extended precision and and mach. precision
    tracker.state.keep_extended_prec = true
    state.last_point .= tracker.state.x
    state.last_t = tracker.state.t
    prediction .= 0.0
    sample_point_acc = Inf
    m = 1
    Δθ = 2π / n₀
    result = CAUCHY_TERMINATED_MAX_WINDING_NUMBER
    while m ≤ options.max_winding_number
        θⱼ = 0.0
        tⱼ = t
        for j = 1:n₀
            θⱼ += Δθ
            tⱼ = j == n₀ ? t : t * cis(θⱼ)
            res = track!(tracker, tⱼ)
            sample_point_acc = tracker.state.accuracy
            prediction_acc = max(prediction_acc, sample_point_acc)

            if !is_success(res)
                result = CAUCHY_TERMINATED
                @goto _return
            end

            prediction .+= x
        end
        # Check that loop is closed
        d = tracker.state.norm(last_point, x)
        if d < 10 * max(prediction_acc, sample_point_acc)
            n = n₀ * m
            prediction .= prediction ./ n

            result = CAUCHY_SUCCESS
            break
        end
        m += 1
    end

    @label _return

    if result == CAUCHY_TERMINATED
        init!(tracker, last_point, t, 0.0; ω = ω, μ = μ, keep_steps = true)
    else
        init!(tracker, 0.0)
    end

    result, m, prediction_acc
end

function max_ratio(u::Vector{Float64}, v::Vector{Float64})
    max_rat = -Inf
    for i in eachindex(u)
        max_rat = max(max_rat, u[i] / v[i])
    end
    max_rat
end
function min_ratio(u::Vector{Float64}, v::Vector{Float64})
    min_rat = Inf
    for i in eachindex(u)
        min_rat = max(min_rat, u[i] / v[i])
    end
    min_rat
end

function step!(endgame_tracker::EndgameTracker, debug::Bool = false)
    @unpack tracker, state, options = endgame_tracker

    proposed_t′ = real(tracker.state.t′)
    state.last_point .= tracker.state.x
    state.last_t = tracker.state.t
    step!(tracker, debug)

    state.code = status(tracker)

    if !is_tracking(state.code)
        state.accuracy = tracker.state.accuracy
        state.solution .= tracker.state.x
        state.winding_number = nothing
        # only update condition number for successfull paths
        if is_success(state.code)
            state.cond =
                LA.cond(tracker.state.jacobian, state.row_scaling, state.col_scaling)
        end

        # If a path got terminated, let's be more generous to classify them as at_infinity
        if state.code == EndgameTrackerCode.terminated_accuracy_limit ||
           state.code == EndgameTrackerCode.terminated_ill_conditioned

            @label terminated
            res = analyze(
                state.val;
                finite_tol = 0.0,
                zero_is_finite = !options.zero_is_at_infinity,
                max_winding_number = options.max_winding_number,
            )

            if options.at_infinity_check &&
               res.at_infinity_tol < cbrt(options.val_at_infinity_tol)
                return (state.code = EndgameTrackerCode.at_infinity)
            elseif options.zero_is_at_infinity &&
                   res.at_zero_tol < cbrt(options.val_at_infinity_tol)
                return (state.code = EndgameTrackerCode.at_zero)
            end
        end
        return state.code
    end

    t = real(tracker.state.t)
    t < options.endgame_start || return state.code

    if !state.endgame_started
        #=
         We enter the endgame.
         We monitor the condition number with a fixed scaling of the columns and rows,
         which is recorded now.
        =#
        state.col_scaling .= weights(tracker.state.norm)
        row_scaling!(state.row_scaling, jacobian(tracker.state.jacobian), state.col_scaling)
        κ = LA.cond(tracker.state.jacobian, state.row_scaling, state.col_scaling)
        state.cond_eg_start = κ

        tracker.state.use_strict_β_τ = true
        state.endgame_started = true
    end

    state.steps_eg += 1

    if state.steps_eg > options.max_endgame_steps
        return (state.code = EndgameTrackerCode.terminated_max_steps)
    end
    if tracker.state.last_step_failed
        state.jump_to_zero_failed = (last(state.jump_to_zero_failed), iszero(proposed_t′))
        return state.code
    else
        state.jump_to_zero_failed = (last(state.jump_to_zero_failed), false)
    end

    update!(state.val, tracker.predictor, t)
    (val_finite, at_infinity_tol, at_zero_tol) = analyze(
        state.val;
        finite_tol = options.val_finite_tol * exp10(-state.cauchy_failures),
        zero_is_finite = !options.zero_is_at_infinity,
        max_winding_number = options.max_winding_number,
    )

    if val_finite || (min(at_zero_tol, at_infinity_tol) < sqrt(options.val_at_infinity_tol))
        state.cond = egcond(tracker.state.jacobian, state.row_scaling, state.col_scaling)
    end

    # For the truncation of paths use the stronger requirement of a relative
    # condition number increase
    κ = state.cond / state.cond_eg_start

    κ_min = options.min_cond

    val_at_infinity_tol = begin
        if κ > κ_min^3
            cbrt(options.val_at_infinity_tol)
        elseif κ > κ_min^2
            sqrt(options.val_at_infinity_tol)
        elseif κ > κ_min
            options.val_at_infinity_tol
        else
            0.0
        end
    end

    at_infinity =
        options.at_infinity_check &&
        at_infinity_tol < val_at_infinity_tol &&
        validate_coord_growth(
            state.val,
            state.col_scaling,
            weights(tracker.state.norm);
            finite_tol = options.val_finite_tol,
            at_infinity_tol = val_at_infinity_tol,
            tol = options.min_coord_growth,
        )
    at_zero =
        options.at_infinity_check &&
        options.zero_is_at_infinity &&
        at_infinity_tol < val_at_infinity_tol &&
        validate_coord_growth(
            state.val,
            weights(tracker.state.norm),
            state.col_scaling;
            finite_tol = options.val_finite_tol,
            at_infinity_tol = val_at_infinity_tol,
            tol = options.min_coord_growth,
        )
    finite = !options.only_nonsingular && val_finite && κ > options.min_cond

    if debug
        color = tracker.state.extended_prec ? :blue : :yellow
        printstyled("t = ", t, "  t′ = ", real(tracker.state.t′), "\n", color = color)
        κ = egcond(tracker.state.jacobian, state.row_scaling, state.col_scaling)
        print("κ = ")
        Printf.@printf("%.3e (%.3e)\n", κ, κ / state.cond_eg_start)
        println(state.val)

        printstyled("Val: ", bold = true)
        if val_finite
            printstyled("val_finite ", color = :blue, bold = true)
        end
        if finite
            printstyled("finite ", color = :blue, bold = true)
        end
        if at_infinity
            printstyled("at_infinity ", color = :blue, bold = true)
        end
        if at_zero
            printstyled("at_zero ", color = :blue, bold = true)
        end
        println("\n")
    end

    if at_infinity
        state.accuracy = tracker.state.accuracy
        state.solution .= tracker.state.x
        return (state.code = EndgameTrackerCode.at_infinity)

    elseif at_zero
        state.accuracy = tracker.state.accuracy
        state.solution .= tracker.state.x
        return (state.code = EndgameTrackerCode.at_zero)

    elseif finite && (isnan(state.t_last_cauchy) || 10t < state.t_last_cauchy)
        res, m, acc_est = cauchy!(state, tracker, options)
        if debug
            printstyled("Cauchy result: ", res, " ", m, " ", acc_est, "\n"; color = :blue)
        end
        if res == CAUCHY_SUCCESS
            if state.winding_number === nothing
                @label save_cauchy_result
                state.t_last_cauchy = t
                state.winding_number = m
                state.solution .= state.prediction
                state.accuracy = acc_est
            elseif state.winding_number != m
                state.cauchy_failures += 1
                @goto save_cauchy_result
            elseif state.winding_number == m
                w = weights(tracker.state.norm)
                d = distance(state.prediction, state.solution, InfNorm()) / maximum(w)
                if d < 100 * max(state.accuracy, acc_est)
                    state.solution .= state.prediction
                    state.accuracy = d
                    state.cond = LA.cond(
                        tracker,
                        state.solution,
                        0.0,
                        state.row_scaling,
                        state.col_scaling,
                    )
                    return (state.code = EndgameTrackerCode.success)
                else
                    state.accuracy = acc_est
                    state.solution .= state.prediction
                    state.cauchy_failures += 1
                    @goto save_cauchy_result
                end
            end
        elseif res == CAUCHY_TERMINATED_MAX_WINDING_NUMBER
            if state.max_winding_number_hit
                state.code = EndgameTrackerCode.terminated_max_winding_number
            else
                state.cauchy_failures += 1
                state.max_winding_number_hit = true
            end
        elseif res == CAUCHY_TERMINATED
            state.code = tracker.state.code
        end
    end

    # Catch ill behavior and terminate the tracking
    # 1) check cond(H_x, ẋ)
    if tracker.predictor.cond_H_ẋ > 1e13 && maximum(state.val.val_tẋ) > 1e2
        state.code = EndgameTrackerCode.terminated_ill_conditioned
        @goto terminated
    end

    state.code
end

function track!(
    endgame_tracker::EndgameTracker,
    x,
    t₁::Real;
    ω::Float64 = NaN,
    μ::Float64 = NaN,
    debug::Bool = false,
)
    init!(endgame_tracker, x, t₁; ω = ω, μ = μ)

    while is_tracking(endgame_tracker.state.code)
        step!(endgame_tracker, debug)
    end

    endgame_tracker.state.code
end

"""
    start_parameters!(tracker::EndgameTracker, p)

Set the start parameters of the homotopy of the tracker.
"""
start_parameters!(T::EndgameTracker, p) = (start_parameters!(T.tracker, p); T)

"""
    target_parameters!(tracker::EndgameTracker, p)

Set the target parameters of the homotopy of the tracker.
"""
target_parameters!(T::EndgameTracker, p) = (target_parameters!(T.tracker, p); T)

function solution(endgame_tracker::EndgameTracker)
    get_solution(endgame_tracker.tracker.homotopy, endgame_tracker.state.solution, 0.0)
end

function PathResult(
    endgame_tracker::EndgameTracker,
    start_solution = nothing,
    path_number = nothing,
)
    @unpack tracker, state, options = endgame_tracker
    H = tracker.homotopy

    if is_success(state.code)
        t = 0.0
        solution = get_solution(H, state.solution, 0.0)
        evaluate!(tracker.corrector.r, H, state.solution, complex(0.0))
        residual = LA.norm(tracker.corrector.r, InfNorm())
    else
        t = real(tracker.state.t)
        solution = get_solution(H, tracker.state.x, t)
        evaluate!(tracker.corrector.r, H, tracker.state.x, complex(t))
        residual = LA.norm(tracker.corrector.r, InfNorm())
    end

    PathResult(
        return_code = Symbol(state.code),
        solution = solution,
        t = t,
        accuracy = state.accuracy,
        residual = residual,
        condition_jacobian = state.cond,
        winding_number = state.winding_number,
        multiplicity = nothing,
        last_path_point = (get_solution(H, state.last_point, state.last_t), state.last_t),
        valuation = t > options.endgame_start ? nothing : copy(state.val.val_x),
        start_solution = start_solution,
        path_number = path_number,
        ω = tracker.state.ω,
        μ = tracker.state.μ,
        extended_precision = tracker.state.extended_prec,
        accepted_steps = tracker.state.accepted_steps,
        rejected_steps = tracker.state.rejected_steps,
        steps_eg = state.steps_eg,
        extended_precision_used = tracker.state.used_extended_prec,
    )
end

"""
    track(endgame_tracker::EndgameTracker, x::AbstractVector, t::Real = 1.0;
          path_number = nothing, debug = false)

Track the given start solution `x` from `t` towards `0` using the given `endgame_tracker`.
Returns a [`PathResult`](@ref).

    track(endgame_tracker::EndgameTracker, r::PathResult, t::Real = 1.0;
          path_number = nothing, debug = false)

Track `solution(r)` from `t` towards `0` using the given `endgame_tracker`.
"""
function track(
    endgame_tracker::EndgameTracker,
    x::AbstractVector,
    t₁::Real = 1.0;
    path_number::Union{Nothing,Int} = nothing,
    kwargs...,
)
    track!(endgame_tracker, x, t₁; kwargs...)
    PathResult(endgame_tracker, x, path_number)
end
function track(endgame_tracker::EndgameTracker, r::PathResult, t₁::Real; kwargs...)
    track(endgame_tracker, solution(r), t₁; ω = r.ω, μ = r.μ, kwargs...)
end


text:
push!(LOAD_PATH, "../src/")

using Documenter, TensorAlgebra

makedocs(sitename="TensorAlgebra.jl",
    clean=true,
    format=Documenter.HTML(
        prettyurls=get(ENV, "CI", nothing) == "true",
        footer=nothing
    )
)

deploydocs(
    repo = "github.com/EricForgy/TensorAlgebra.jl.git",
)


text:
import Robotlib: ad, adi
function calibLPOE(xin,Tn0in,Ta,q;maxiter=10, λ=1.0)

    xi      = deepcopy(xin)
    Tn0     = deepcopy(Tn0in)
    n       = size(xi,2)-1
    N       = size(q,1)
    y       = zeros(6N)
    A       = zeros(6N,6*(n+1))
    xini    = deepcopy(xin)
    Tn0cand = deepcopy(Tn0)
    v       = zeros(3) # only revolute joints
    et      = zeros(maxiter+1)
    er      = zeros(maxiter+1)
    et[1],er[1]   = evalError(xi,Tn0,Ta,q)
    println("Error: ",et[1], " λ: ", λ)
    @assert size(Ta,3) == N

    for iter = 1:maxiter # do a few iterations of the calibration

        for i = 1:N
            Tfull = fkineLPOE(Tn0,xi,q[i,:]')
            # populate the matrices of the linear estimation problem
            y[(i-1)*6+1:6i] = twistcoords(logT(Ta[:,:,i]*trinv(Tfull)))
            A[(i-1)*6+1:6*i,1:6] = ad(Tn0[:,:,1])
            adi = ad(Tn0[:,:,1]*expξ(xi[:,1],q[i,1]))
            for j = 2:n
                A[(i-1)*6+1:6*i,(j-1)*6+1:6*j] = adi*ad(Tn0[:,:,j])
                adi = adi*ad(Tn0[:,:,j]*expξ(xi[:,j],q[i,j]))
            end
            A[(i-1)*6+1:6*i,n*6+1:6*(n+1)] = adi*ad(Tn0[:,:,n+1])
        end
        x = (A'A + λ*I)\A'y
        # Update the candidate nominal parameters
        for j = 1:n+1
            ξ = x[(j-1)*6+1:6j]
            Tn0cand[:,:,j] = Tn0[:,:,j]*expξ2(ξ,1)
        end

        et[iter+1],er[iter+1] = evalError(xi,Tn0cand,Ta,q)
        println("Error: ",round(et[iter+1],5), " λ: ", λ, " Norm dx: ", round(norm(x),5))

        if norm(x) < 1e-16
            return Tn0,xi, et, er
        end
        if et[iter+1] > et[iter]
            et[iter+1] = et[iter]
            er[iter+1] = er[iter]
            λ *= 10
        else
            λ/=10
            xi = deepcopy(xini)
            Tn0 = deepcopy(Tn0cand)
        end

    end

    return Tn0, xi, et, er

end

"""
`calibLPOEdual(xi,Tn0,q;maxiter=10, λ=1.0)`
Performs dual arm calibration given joint twists xi and nominal transformations Tn0
Returns calibrated nominal transformations
"""
function calibLPOEdual(xin,Tn0in,q;maxiter=10, λ=1.0)

    xi      = deepcopy(xin)
    Tn0     = deepcopy(Tn0in)
    n       = size(xi,2)-1
    N       = size(q,1)
    y       = zeros(6N)
    A       = zeros(6N,6*(n+1),2)
    xini    = deepcopy(xin)
    Tn0cand = deepcopy(Tn0)
    et      = zeros(maxiter+1)
    er      = zeros(maxiter+1)
    et[1],er[1]   = evalErrorDual(xi,Tn0,q)
    println("Error: ",et[1], " λ: ", λ)

    for iter = 1:maxiter # do a few iterations of the calibration
        for i = 1:N
            T1 = fkineLPOE(Tn0[:,:,:,1],xi[:,:,1],q[i,:,1]')
            T2 = fkineLPOE(Tn0[:,:,:,2],xi[:,:,2],q[i,:,2]') # Use the other arm as measurement
            y[(i-1)*6+1:6i] = twistcoords(logT(T2*trinv(T1)))
            for arm = 1:2
                A[(i-1)*6+1:6*i,1:6,arm] = ad(Tn0[:,:,1,arm])
                adi = ad(Tn0[:,:,1,arm]*expξ(xi[:,1,arm],q[i,1,arm]))
                for j = 2:n
                    A[(i-1)*6+1:6*i,(j-1)*6+1:6*j,arm] = adi*ad(Tn0[:,:,j,arm])
                    adi = adi*ad(Tn0[:,:,j,arm]*expξ(xi[:,j,arm],q[i,j,arm]))
                end
                A[(i-1)*6+1:6*i,n*6+1:6*(n+1),arm] = adi*ad(Tn0[:,:,n+1,arm])

            end
        end
        x = zeros(3)
        for arm = 1:2
            x = (A[:,:,arm]'A[:,:,arm] + λ*I)\A[:,:,arm]'y
            # Update the candidate nominal parameters
            s = arm == 1 ? 1 : -1 # Determine the sign of the correction
            for j = 1:n+1
                ξ = 0.5*s*x[(j-1)*6+1:6j] # Only apply half the update to each arm, verified to be essential
                Tn0cand[:,:,j,arm] = Tn0[:,:,j,arm]*expξ2(ξ,1)
            end

        end
        et[iter+1],er[iter+1] = evalErrorDual(xi,Tn0cand,q)
        println("Error: ",round(et[iter+1],5), " λ: ", λ, " Norm dx: ", round(norm(x),5))

        if norm(x) < 1e-14
            return Tn0,xi, et, er
        end
        if et[iter+1] > et[iter]
            et[iter+1] = et[iter]
            er[iter+1] = er[iter]
            λ *= 10
        else
            λ/=10
            xi = deepcopy(xini)
            Tn0 = deepcopy(Tn0cand)
        end

    end

    return Tn0, xi, et, er

end

function calibPOE(Xin,Ta,q;maxiter=50, λ = 10000.0)

    xin     = copy(Xin)
    n       = size(xin,2)-2
    N       = size(q,1)
    y       = zeros(6N)
    A       = zeros(6N,6*(n+1))
    xini    = similar(xin)
    et      = zeros(maxiter+1)
    er      = zeros(maxiter+1)
    et[1],er[1]   = evalErrorPOE(xin,Ta,q)
    println("Error: ",et[1], " λ: ", λ)
    @assert size(Ta,3) == N

    for iter = 1:maxiter # do a few iterations of the calibration

        for i = 1:N
            Tfull = fkinePOE(xin,q[i,:]')
            # populate the matrices of the linear estimation problem
            y[(i-1)*6+1:6i] = twistcoords(logT(Ta[:,:,i]*trinv(Tfull)))
            A[(i-1)*6+1:6*i,:] = Ai(q[i,:]',xin)

        end

        x = (A'A + λ*I)\A'y
        # Update the candidate nominal parameters
        for j = 1:n+1
            ξ = x[(j-1)*6+1:6j]
            xini[:,j] = xin[:,j] + ξ
            #xini[:,j] = conformize(xini[:,j])
        end

        et[iter+1],er[iter+1] = evalErrorPOE(xini, Ta,q)
        println("Error: ",round(et[iter+1],5), " λ: ", λ, " Norm dx: ", round(norm(x),5))

        if norm(x) < 1e-10
            return xin, et, er
        end
        if et[iter+1] > et[iter]
            et[iter+1] = et[iter]
            λ *= 10
        else
            λ/=10
            xin = deepcopy(xini)
        end

    end

    return xin, et, er

end

function calibPOE_offsets_from_points(Xin,Q;maxiter=50, λ = 10000.0)
    xin     = copy(Xin)
    n       = size(xin,2)-2
    Ndatasets = size(Q,1)
    Npoints = sum(map(x-> size(x,1),Q))

    y       = zeros(6Npoints)
    A       = zeros(6Npoints,n-1)
    xini    = similar(xin)
    et      = zeros(maxiter+1)
    er      = zeros(maxiter+1)
    et[1]   = evalErrorPOE_offsets_from_points(xin,Q, zeros(n-1))
    println("Error: ",et[1], " λ: ", λ)
    δq      = zeros(n-1)

    for iter = 1:maxiter # do a few iterations of the calibration

        ii = 1

        for p = 1:Ndatasets
            q = Q[p]
            N = size(q,1)
            Tfull = cat(3,[fkinePOE(xin,q[i,:]'+[δq;0]) for i = 1:N]...)
            Tm = squeeze(mean(Tfull,3),3)
            toOrthoNormal!(Tm)
            for i = 1:N
                # populate the matrices of the linear estimation problem
                y[ii:ii+5] = twistcoords(logT(Tm*trinv(Tfull[:,:,i])))
                A[ii:ii+5,:] = xii(q[i,1:(n-1)]'+δq, xin[:,1:(n-1)])
                ii += 6
            end
        end

        x = (A'A + λ*I)\A'y
        # Update the candidate nominal parameters
        δqi = δq + x

        et[iter+1] = evalErrorPOE_offsets_from_points(xin, Q, δqi)
        println("$iter Error: ",round(et[iter+1],5), " λ: ", λ, " Norm dx: ", round(norm(x),5))

        if norm(x) < 1e-10
            return δq, et
        end
        if et[iter+1] > et[iter]
            et[iter+1] = et[iter]
            λ *= 10
        else
            # λ = max(λ/10,1e-8)
            δq = deepcopy(δqi)
        end

    end

    return δq, et

end

function evalError(xin,Tn0,Ta,q)
    N = size(Ta,3)
    et = 0.0
    er = 0.0
    for i = 1:N
        Tfull = fkineLPOE(Tn0,xin,q[i,:]')
        et += norm(Ta[1:3,4,i]-Tfull[1:3,4])
        er += norm(skewcoords(logR(Ta[1:3,1:3,i]*Tfull[1:3,1:3]')))
    end
    return et/N, er/N
end

function evalErrorDual(xin,Tn0,q)
    N = size(Tn0,3)
    et = 0.0
    er = 0.0
    for i = 1:N
        T1 = fkineLPOE(Tn0[:,:,:,1],xin[:,:,1],q[i,:,1]')
        T2 = fkineLPOE(Tn0[:,:,:,2],xin[:,:,2],q[i,:,2]')
        et += norm(T2[1:3,4]-T1[1:3,4])
        er += norm(skewcoords(logR(T2[1:3,1:3]*T1[1:3,1:3]')))
    end
    return et/N, er/N
end

function evalErrorPOE(xin,Ta,q)
    xini = copy(xin)
    N = size(Ta,3)
    et = 0.0
    er = 0.0
    for i = 1:N
        Tfull = fkinePOE(xin,q[i,:]')
        et += norm(Ta[1:3,4,i]-Tfull[1:3,4])
        er += norm(skewcoords(logR(Ta[1:3,1:3,i]*Tfull[1:3,1:3]')))
    end
    return et/N, er/N
end

function evalErrorPOE_offsets_from_points(xi,Q,dq)
    Ndatasets = size(Q,1)
    J = Array{Float64}(Ndatasets)
    for p = 1:Ndatasets
        q = Q[p]
        N = size(q,1)
        T = Array{typeof(dq[1])}(4,4,N)
        for i = 1:N
            T[:,:,i] = fkinePOE(xi,q[i,:]' + [dq;0])
        end
        xyz = squeeze(T[1:3,4,:],2)
        mxyz = mean(xyz,2)
        Jpos = sum(abs((xyz.-mxyz)./mxyz))/N

        Rm = squeeze(mean(T[1:3,1:3,:],3),3)
        toOrthoNormal!(Rm)
        Jori = 0.0
        @inbounds for i = 1:N
            Jori += Rangle(T[1:3,1:3,i],Rm)
        end
        Jori /= Rangle(Rm)
        Jori /= N

        J[p] = Jpos + Jori
    end
    return sum(J)
end

function simulateCalibration1(N)
    n       = 6
    q       = 2π*rand(N,n)

    wn      = zeros(3,n)
    pn      = zeros(3,n)
    xin     = zeros(6,n+1)
    wn[:,1] = [0,0,1]
    pn[:,1] = [0,0,0]
    wn[:,2] = [0,1,0]
    pn[:,2] = [0,0,0.3]
    wn[:,3] = [0,1,0]
    pn[:,3] = [0,0,1]
    wn[:,4] = [1,0,0]
    pn[:,4] = [1,0,1]
    wn[:,5] = [0,1,0]
    pn[:,5] = [1,0,1]
    wn[:,6] = [1,0,0]
    pn[:,6] = [1,0,1]
    T0 = [  0 -1 0  1.2;
    0 0 -1 0;
    1 0  0  1;
    0 0  0  1]
    for i = 1:n
        xin[:,i] = [-skew(pn[:,i])*wn[:,i]; wn[:,i]]
    end
    xin[:,n+1] = twistcoords(logT(T0))
    xinmod   = deepcopy(xin)
    for i = 1:n+1
        xinmod[:,i]  += [0.001randn(3); 0.01π/180*randn(3)]
        #conformize(xinmod[:,i])
    end
    Ta  = zeros(4,4,N)
    for i = 1:N
        Ta[:,:,i] = fkinePOE(xin,q[i,:]')
    end
    return q, xin, T0, xinmod, Ta
end

function simulateCalibration2(N)
    n       = 6
    q       = 2π*rand(N,n)
    dh      = DH7600()
    AAA,BBB,T0,Ti0,Tn0 = jacobian(zeros(6),dh, eye(4));
    xin     = DH2twistsPOE(dh)
    xinmod   = deepcopy(xin)
    for i = 1:n+1
        xinmod[:,i]  += [0.001randn(3); 0.01π/180*randn(3)]
        #conformize(xinmod[:,i])
    end
    Ta  = zeros(4,4,N)
    for i = 1:N
        Ta[:,:,i] = fkinePOE(xin,q[i,:]')
    end
    return q, xin, T0, xinmod, Ta
end

function simulateCalibration3(N)
    srand(1)
    n       = 6
    q       = 2π*rand(N,n)
    dh      = DH7600()
    AAA,BBB,T0,Ti0,Tn0 = jacobian(zeros(6),dh, eye(4));
    xin = DH2twistsLPOE(Tn0)
    Tn0mod = deepcopy(Tn0)
    for i = 1:n+1
        Tn0mod[1:3,4,i] += 0.1randn(3)
    end
    Ta  = zeros(4,4,N)
    for i = 1:N
        Ta[:,:,i] = fkineLPOE(Tn0,xin,q[i,:]')
        AAA,BBB,T = jacobian(q[i,:]',dh, eye(4));
        @assert T ≈ Ta[:,:,i]
    end
    return q, xin, Tn0, Tn0mod, Ta
end

function simulateCalibration4(N)
    #srand(1)
    n       = 6
    qt      = 2π*rand(N,n)
    q1      = qt + 0.01*π/180*randn(N,n)
    q2      = qt + 0.01*π/180*randn(N,n)
    q       = cat(3,q1,q2)
    dh      = DH7600()
    AAA,BBB,T0,Ti0,Tn0 = jacobian(zeros(6),dh, eye(4));
    xin = DH2twistsLPOE(Tn0)
    xin = cat(3,xin,xin)
    Tn0mod = cat(4,deepcopy(Tn0),deepcopy(Tn0))
    for i = 1:n+1
        Tn0mod[1:3,4,i,1] += 0.1randn(3)
        Tn0mod[1:3,4,i,2] += 0.1randn(3)
    end
    Ta  = zeros(4,4,N)
    for i = 1:N
        Ta[:,:,i] = fkineLPOE(Tn0,xin,qt[i,:]')
    end

    return q, xin, Tn0, Tn0mod, Ta
end

# if false
#     pathopen    = "/work/fredrikb/flexifab/logs/calibrationNikon/2400_flexifabseamtracking.csv"
#     pathsave    = "/tmp/fredrikb/log.mat"
#     orcalog2mat(pathopen, pathsave)
#     ds          = 100
#     data        = MAT.matread(pathsave)
#     q           = getData("j_act_j*",data, ds, removeNaN = false)
#     q_frame     = getData("q_frame*",data, ds, removeNaN = false)
#
#     nans = any(isnan(q),2) | any(isnan(q_frame),2)
#
#     q = q[!nans,:]
#     q_frame = q_frame[!nans,:]
#     # plot(q)
#     using Quaternions
#
#     N = size(q,1)
#     Ta = zeros(4,4,N)
#     for i = 1:N
#         t = q_frame[i,6:8]'/1000
#         R = rotationmatrix(Quaternion(q_frame[i,1:4]...))
#         @assert isrot(R)
#         Ta[:,:,i] = [R t;0 0 0 1]
#     end
#
# end
if false
    q, xin,T0, xinmod, Ta= Robotlib.Calibration.simulateCalibration2(100)
    xic,et,er = calibPOE(xinmod,Ta,q,maxiter=150, λ = 10000.0)
    display(norm(xin[:,1:7]-xinmod[:,1:7]))
    display(norm(xin[:,1:7]-xic[:,1:7]))
end

if false
    q, xin,Tn0,Tn0mod, Ta = simulateCalibration3(100)
    @time Tn0c,xic,et,er = calibLPOE(xin,Tn0mod,Ta,q,maxiter=10, λ = 100.0)
    display(sqrt(sum((Tn0-Tn0mod).^2)/100))
    display(sqrt(sum((Tn0-Tn0c).^2)/100))
    display(round(Tn0-Tn0c,5))
end

if false
    N = 100
    q, xin,Tn0,Tn0mod, Ta = simulateCalibration4(N)
    @time Tn0c,xic,et,er = calibLPOEdual(xin,Tn0mod,q,maxiter=6, λ = 0.01)
    println("Error between Tn0 and Tn0mod: ",sqrt(sum((Tn0[1:3,4,:]-Tn0mod[1:3,4,:,1]).^2+(Tn0[1:3,4,:]-Tn0mod[1:3,4,:,2]).^2)/N))
    println("Error between Tn0 and Tn0c  : ",sqrt(sum((Tn0[1:3,4,:]-Tn0c[1:3,4,:,1]).^2+(Tn0[1:3,4,:]-Tn0c[1:3,4,:,2]).^2)/N))
    ei = 0.0
    ec = 0.0
    for i = 1:N
        T1 = fkineLPOE(Tn0mod,xin,q[i,:,1]')
        T2 = fkineLPOE(Tn0mod,xin,q[i,:,2]')
        ei += norm(twistcoords(logm(Ta[:,:,i]*trinv(T1))))
        ei += norm(twistcoords(logm(Ta[:,:,i]*trinv(T2))))
        T1 = fkineLPOE(Tn0c,xin,q[i,:,1]')
        T2 = fkineLPOE(Tn0c,xin,q[i,:,2]')
        ec += norm(twistcoords(logm(Ta[:,:,i]*trinv(T1))))
        ec += norm(twistcoords(logm(Ta[:,:,i]*trinv(T2))))
    end
    println("Initial error: ",round(ei/N,5), " Calibrated error: ", round(ec/N,5))
end

#plot(et[et .!= 0])


text:
module Blosc
using Blosc_jll
export compress, compress!, decompress, decompress!

function __init__()
    ccall((:blosc_init,libblosc), Cvoid, ())
    atexit() do
        ccall((:blosc_destroy,libblosc), Cvoid, ())
    end
end

# The following constants should match those in blosc.h
const VERSION_FORMAT = 2
const MAX_OVERHEAD = 16
const DOSHUFFLE = 0x1
const MEMCPYED = 0x2
const MAX_THREADS = 256

# Blosc is currently limited to 32-bit buffer sizes (Blosc/c-blosc#67)
const MAX_BUFFERSIZE = typemax(Cint) - MAX_OVERHEAD
const MAX_TYPESIZE = 255

using Base.GC: @preserve

# low-level functions:
blosc_compress(level, shuffle, itemsize, srcsize, src, dest, destsize) =
    ccall((:blosc_compress,libblosc), Cint,
          (Cint,Cint,Csize_t, Csize_t, Ptr{Cvoid}, Ptr{Cvoid}, Csize_t),
          level, shuffle, itemsize, srcsize, src, dest, destsize)

blosc_decompress(src, dest, destsize) =
    ccall((:blosc_decompress,libblosc), Cint,
          (Ptr{Cvoid},Ptr{Cvoid},Csize_t), src, dest, destsize)

# check whether the strides of A correspond to contiguous data
iscontiguous(::Array) = true
iscontiguous(::Vector) = true
iscontiguous(A::DenseVector) = stride(A,1) == 1
function iscontiguous(A::DenseArray)
    p = sortperm([strides(A)...])
    s = 1
    for k = 1:ndims(A)
        if stride(A,p[k]) != s
            return false
        end
        s *= size(A,p[k])
    end
    return true
end

# Returns the size of compressed data inside dest
function compress!(dest::DenseVector{UInt8},
                   src::Ptr{T},
                   src_size::Integer;
                   level::Integer=5,
                   shuffle::Bool=true,
                   itemsize::Integer=sizeof(T)) where {T}
    iscontiguous(dest) || throw(ArgumentError("dest must be contiguous array"))
    if !isbitstype(T)
        throw(ArgumentError("buffer eltype must be `isbits` type"))
    end
    if itemsize <= 0
        throw(ArgumentError("itemsize must be positive"))
    end
    if level < 0 || level > 9
        throw(ArgumentError("invalid compression level $level not in [0,9]"))
    end
    if src_size > MAX_BUFFERSIZE
        throw(ArgumentError("data > $MAX_BUFFERSIZE bytes is not supported by Blosc"))
    end
    sz = blosc_compress(level, shuffle, itemsize, src_size, src, dest, sizeof(dest))
    sz < 0 && error("Blosc internal error when compressing data (errorcode: $sz)")
    return convert(Int, sz)
end

compress!(dest::DenseVector{UInt8}, src::AbstractString; kws...) =
    @preserve src compress!(dest, pointer(src), sizeof(src); kws...)

function compress!(dest::DenseVector{UInt8}, src::DenseArray; kws...)
    iscontiguous(src) || throw(ArgumentError("src must be a contiguous array"))
    return @preserve src compress!(dest, pointer(src), sizeof(src); kws...)
end

function compress(src::Ptr{T}, src_size::Integer; kws...) where {T}
    dest = Vector{UInt8}(undef, src_size + MAX_OVERHEAD)
    sz = compress!(dest,src,src_size; kws...)
    @assert(sz > 0 || src_size == 0)
    return resize!(dest, sz)
end
function compress(src::DenseArray; kws...)
    iscontiguous(src) || throw(ArgumentError("src must be a contiguous array"))
    @preserve src compress(pointer(src), sizeof(src); kws...)
end
compress(src::AbstractString; kws...) = @preserve src compress(pointer(src), sizeof(src); kws...)

"""
    compress(data; level=5, shuffle=true, itemsize)

Return a `Vector{UInt8}` of the Blosc-compressed `data`, where `data`
is an array or a string.

The `level` keyword indicates the compression level
(between `0`=no compression and `9`=max), `shuffle` indicates whether to use
Blosc's shuffling preconditioner, and the shuffling preconditioner
is optimized for arrays of binary items of size (in bytes) `itemsize` (defaults
to `sizeof(eltype(data))` for arrays and the size of the code units for strings).
"""
compress

"""
    compress!(dest::Vector{UInt8}, src; kws...)

Like `compress(src; kws...)`, but writes to a pre-allocated array `dest`
of bytes.   The return value is the size in bytes of the data written
to `dest`, or `0` if the buffer was too small.
"""
compress!

# this unexported function is used by the HDF5.jl blosc filter
function cbuffer_sizes(buf)
    s1 = Ref{Csize_t}()
    s2 = Ref{Csize_t}()
    s3 = Ref{Csize_t}()
    ccall((:blosc_cbuffer_sizes,libblosc), Cvoid,
          (Ptr{UInt8}, Ref{Csize_t}, Ref{Csize_t}, Ref{Csize_t}),
          buf, s1, s2, s3)
    return (s1[], s2[], s3[])
end

"""
    sizes(buf::Vector{UInt8})

Given a compressed buffer `buf`, return a tuple
of the `(uncompressed, compressed, block)` sizes in bytes.
"""
sizes(buf::DenseVector{UInt8}) = cbuffer_sizes(buf)

"""
    decompress!(dest::Vector{T}, src::Vector{UInt8})

Like `decompress`, but uses a pre-allocated destination buffer `dest`,
which is resized as needed to store the decompressed data from `src`.
"""
function decompress!(dest::DenseVector{T}, src::DenseVector{UInt8}) where {T}
    if !iscontiguous(dest) || !iscontiguous(src)
        throw(ArgumentError("src and dest must be contiguous arrays"))
    end
    if !isbitstype(T)
        throw(ArgumentError("dest must be a DenseVector of `isbits` element types"))
    end
    uncompressed, = sizes(src)
    if uncompressed == 0
        return resize!(dest, 0)
    end
    sizeT = sizeof(T)
    len = div(uncompressed, sizeT)
    if len * sizeT != uncompressed
        error("uncompressed data is not a multiple of sizeof($T)")
    end
    resize!(dest, len)
    sz = blosc_decompress(src, dest, sizeof(dest))
    sz == 0 && error("Blosc decompress error, compressed data is corrupted")
    sz <  0 && error("Blosc decompress error, output buffer is not large enough")
    return dest
end
"""
    decompress(T::Type, src::Vector{UInt8})

Return the compressed buffer `src` as an array of element type `T`.
"""
decompress(::Type{T}, src::DenseVector{UInt8}) where {T} =
    decompress!(Vector{T}(undef, 0), src)

# Initialize a pool of threads for compression / decompression.
# If `nthreads` is 1, the the serial version is chosen and a possible previous existing pool is ended.
# If this function is not callled, `nthreads` is set to 1 internally.
"""
    set_num_threads(n=Sys.CPU_CORES)

Tells Blosc to use `n` threads for compression/decompression.   If this
function is never called, the default is `1` (serial).
"""
function set_num_threads(n::Integer=Sys.CPU_CORES)
    1 <= n <= MAX_THREADS || throw(ArgumentError("must have 1 ≤ nthreads ≤ $MAX_THREADS"))
    return ccall((:blosc_set_nthreads,libblosc), Cint, (Cint,), n)
end

"""
    set_compressor(s::AbstractString)

Set the current compression algorithm to `s`.  The currently supported
algorithms in the default Blosc module build are `"blosclz"`, `"lz4"`,
and `"l4hc"`.   (Throws an `ArgumentError` if `s` is not the name
of a supported algorithm.)  Returns a nonnegative integer code used
internally by Blosc to identify the compressor.
"""
function set_compressor(s::AbstractString)
    compcode = ccall((:blosc_set_compressor,libblosc), Cint, (Cstring,), s)
    compcode == -1 && throw(ArgumentError("unrecognized compressor $s"))
    return compcode
end

"""
    set_blocksize(blocksize=0)

Force the use of a specific compression `blocksize`. If `0` (the default), an
appropriate blocksize will be chosen automatically by blosc.
"""
function set_blocksize(blocksize::Integer=0)
    blocksize >= 0 || throw(ArgumentError("n must be ≥ 0 (default)"))
    ccall((:blosc_set_blocksize,libblosc), Cvoid, (Csize_t,), blocksize)
end
@deprecate set_default_blocksize() set_blocksize()

"""
    compressor_name(src::Vector{UInt8})

Given a compressed array `src`, returns the name (string) of the
compression library that was used to generate it.  (This is not
the same as the name of the compression algorithm.)
"""
function compressor_library(src::DenseVector{UInt8})
    iscontiguous(src) || throw(ArgumentError("src must be a contiguous array"))
    nptr = ccall((:blosc_cbuffer_complib,libblosc), Ptr{UInt8}, (Ptr{UInt8},), src)
    nptr == convert(Ptr{UInt8}, 0) && error("unknown compression library")
    name = unsafe_string(nptr)
    return name
end
@deprecate compression_library(src::DenseVector{UInt8}) compressor_library(src)

struct CompressionInfo
    library::String
    typesize::Int
    pure_memcopy::Bool
    shuffled::Bool
end

"""
    compressor_info(src::Vector{UInt8})

Given a compressed array `src`, returns the information about the
compression algorithm used in a `CompressionInfo` data structure.
"""
function compressor_info(cbuf::DenseVector{UInt8})
    iscontiguous(cbuf) || throw(ArgumentError("cbuf must be contiguous array"))
    flag, typesize = Cint[0], Csize_t[0]
    ccall((:blosc_cbuffer_metainfo, libblosc), Cvoid,
          (Ptr{Cvoid},Ptr{Csize_t},Ptr{Cint}), cbuf, typesize, flag)
    pure_memcopy = flag[1] & MEMCPYED != 0
    shuffled = flag[1] & DOSHUFFLE != 0
    return CompressionInfo(compressor_library(cbuf),
                           typesize[1],
                           pure_memcopy,
                           shuffled)
end

"""
    compressors()

Return the list of compression algorithms in the Blosc library build
as an array of strings.
"""
compressors() = split(unsafe_string(ccall((:blosc_list_compressors, libblosc), Ptr{UInt8}, ())), ',')

if isdefined("", :data)
    take_cstring(ptr) = unsafe_wrap(String, ptr, true)
else
    function take_cstring(ptr)
        str = unsafe_string(ptr)
        ccall(:free, Cvoid, (Ptr{Cvoid},), ptr)
        return str
    end
end

"""
    compressor_info(name::AbstractString)

Given the `name` of a compressor in the Blosc library, return a tuple
`(compressor name, library name, version number)`.
"""
function compressor_info(name::AbstractString)
    lib, ver = Ref{Ptr{UInt8}}(), Ref{Ptr{UInt8}}()
    ret = ccall((:blosc_get_complib_info, libblosc), Cint,
                (Cstring,Ptr{Ptr{UInt8}},Ptr{Ptr{UInt8}}),
                name, lib, ver)
    ret < 0 && error("Error retrieving compressor info for $name")
    lib_str = take_cstring(lib[])
    ver_str = take_cstring(ver[])
    return (name, lib_str, ver_str == "unknown" ? v"0.0.0" : VersionNumber(ver_str))
end

"""
    compressors_info()

Return an array of tuples `(compressor name, library name, version number)`
for all of the compression libraries included in the Blosc library build.
"""
compressors_info() = map(compressor_info, compressors())

"""
    free_resources!()

Free possible memory temporaries and thread resources.
Use this when you are not going to use Blosc for a long while.
In case of problems releasing resources, it returns `false`,
whereas it returns `true` on success.
"""
free_resources!() = ccall((:blosc_free_resources,libblosc), Cint, ()) == 0

end # module