text:
module WilsonFermion_module
    using LinearAlgebra
    #using ..Fermion:FermionFields

    import ..Actions:FermiActionParam,FermiActionParam_Wilson,
                FermiActionParam_WilsonClover,FermiActionParam_Staggered
    import ..Gaugefields:GaugeFields,GaugeFields_1d,SU3GaugeFields,SU2GaugeFields,SU3GaugeFields_1d,SU2GaugeFields_1d,
                staggered_phase,SUn,SU2,SU3,SUNGaugeFields,SUNGaugeFields_1d,SU

    #import ..Fermion:FermionFields,
    #            clear!
    #using ..Fermion
    import ..AbstractFermion:FermionFields,
        Wx!,Wdagx!,clear!,substitute_fermion!,Dx!,fermion_shift!,fermion_shiftB!,add!,set_wing_fermi!,WdagWx!,apply_periodicity,Ddagx!

    """
    Struct for WilsonFermion
    """
    struct WilsonFermion <: FermionFields
        NC::Int64
        NX::Int64
        NY::Int64
        NZ::Int64
        NT::Int64
        f::Array{ComplexF64,6}

        γ::Array{ComplexF64,3}
        rplusγ::Array{ComplexF64,3}
        rminusγ::Array{ComplexF64,3}
        hop::Float64 #Hopping parameter
        r::Float64 #Wilson term
        hopp::Array{ComplexF64,1}
        hopm::Array{ComplexF64,1}
        eps::Float64
        Dirac_operator::String
        MaxCGstep::Int64
        BoundaryCondition::Array{Int8,1}
    end

    function Base.length(x::WilsonFermion)
        return x.NC*x.NX*x.NY*x.NZ*x.NT*4
    end

    function Base.size(x::WilsonFermion)
        return (x.NC,x.NX,x.NY,x.NZ,x.NT,4)
    end

    function Base.iterate(x::WilsonFermion, i::Int = 1)
        i == length(x.f)+1 && return nothing
        #println("i = $i ",x.f[i])
        return (x.f[i], i+1)
    end

    function WilsonFermion(NC,NX,NY,NZ,NT,fparam::FermiActionParam,BoundaryCondition)
        r = fparam.r
        hop = fparam.hop
        eps = fparam.eps
        MaxCGstep = fparam.MaxCGstep
        return WilsonFermion(NC,NX,NY,NZ,NT,r,hop,eps,MaxCGstep,BoundaryCondition)
    end

    function WilsonFermion(NC,NX,NY,NZ,NT,r,hop,eps,MaxCGstep,BoundaryCondition)#r,hop,eps,MaxCGstep)
        γ,rplusγ,rminusγ = mk_gamma(r)
        hopp = zeros(ComplexF64,4)
        hopm = zeros(ComplexF64,4)
        hopp .= hop
        hopm .= hop
        Dirac_operator = "Wilson"
        return WilsonFermion(NC,NX,NY,NZ,NT,zeros(ComplexF64,NC,NX+2,NY+2,NZ+2,NT+2,4),
            γ,rplusγ,rminusγ,hop,r,hopp,hopm,eps,Dirac_operator,MaxCGstep,BoundaryCondition)
    end

    function WilsonFermion(NC,NX,NY,NZ,NT,γ,rplusγ,rminusγ,hop,r,hopp,hopm,eps,fermion,MaxCGstep,BoundaryCondition)
        return WilsonFermion(NC,NX,NY,NZ,NT,zeros(ComplexF64,NC,NX+2,NY+2,NZ+2,NT+2,4),
                    γ,rplusγ,rminusγ,hop,r,hopp,hopm,eps,fermion,MaxCGstep,BoundaryCondition)
    end

    function Base.setindex!(x::WilsonFermion,v,i1,i2,i3,i4,i5,i6)
        x.f[i1,i2 + 1,i3 + 1,i4 + 1,i5 + 1,i6] = v
    end

    function Base.setindex!(x::WilsonFermion,v,ii)
        #ii = (((((ialpha -1)*NT+it-1)*NZ+iz-1)*NY+iy-1)*NX+ix-1)*NC+ic
        ic = (ii - 1) % x.NC + 1
        iii = (ii - ic) ÷ x.NC
        ix = iii % x.NX + 1
        iii = (iii - ix + 1) ÷ x.NX
        iy = iii % x.NY + 1
        iii = (iii - iy + 1) ÷ x.NY
        iz = iii % x.NZ + 1
        iii = (iii - iz + 1) ÷ x.NZ
        it = iii % x.NT + 1
        iii = (iii - it + 1) ÷ x.NT
        ialpha = iii + 1
        x[ic,ix,iy,iz,it,ialpha] = v
    end

    function Base.getindex(x::WilsonFermion,i1,i2,i3,i4,i5,i6)
        return x.f[i1,i2 .+ 1,i3 .+ 1,i4 .+ 1,i5 .+ 1,i6]
    end

    function Base.getindex(x::WilsonFermion,ii)
        #ii = (((((ialpha -1)*NT+it-1)*NZ+iz-1)*NY+iy-1)*NX+ix-1)*NC+ic
        ic = (ii - 1) % x.NC + 1
        iii = (ii - ic) ÷ x.NC
        ix = iii % x.NX + 1
        iii = (iii - ix + 1) ÷ x.NX
        iy = iii % x.NY + 1
        iii = (iii - iy + 1) ÷ x.NY
        iz = iii % x.NZ + 1
        iii = (iii - iz + 1) ÷ x.NZ
        it = iii % x.NT + 1
        iii = (iii - it + 1) ÷ x.NT
        ialpha = iii + 1
        return x[ic,ix,iy,iz,it,ialpha]
    end

    function Base.:*(a::WilsonFermion,b::WilsonFermion)
        c = 0.0im
        for α=1:4
            for it=1:a.NT
                for iz=1:a.NZ
                    for iy=1:a.NY
                        for ix=1:a.NX
                            @simd for ic=1:a.NC
                                c+= conj(a[ic,ix,iy,iz,it,α])*b[ic,ix,iy,iz,it,α]
                            end
                        end
                    end
                end
            end
        end
        return c
    end

    function Base.:*(a::T,b::WilsonFermion) where T <: Number
        c = similar(b)
        for α=1:4
            for it=1:b.NT
                for iz=1:b.NZ
                    for iy=1:b.NY
                        for ix=1:b.NX
                            @simd for ic=1:b.NC
                                c[ic,ix,iy,iz,it,α] = a*b[ic,ix,iy,iz,it,α]
                            end
                        end
                    end
                end
            end
        end
        return c
    end

    function LinearAlgebra.axpy!(a::T,X::WilsonFermion,Y::WilsonFermion) where T <: Number #Y = a*X+Y
        for α=1:4
            for it=1:X.NT
                for iz=1:X.NZ
                    for iy=1:X.NY
                        for ix=1:X.NX
                            @simd for ic=1:X.NC
                                Y[ic,ix,iy,iz,it,α] += a*X[ic,ix,iy,iz,it,α]
                            end
                        end
                    end
                end
            end
        end
        return X
    end

    function LinearAlgebra.rmul!(a::WilsonFermion,b::T) where T <: Number
        for α=1:4
            for it=1:a.NT
                for iz=1:a.NZ
                    for iy=1:a.NY
                        for ix=1:a.NX
                            @simd for ic=1:a.NC
                                a[ic,ix,iy,iz,it,α] = b*a[ic,ix,iy,iz,it,α]
                            end
                        end
                    end
                end
            end
        end
        return a
    end

    function Base.similar(x::WilsonFermion)
        return WilsonFermion(x.NC,x.NX,x.NY,x.NZ,x.NT,
                    x.γ,x.rplusγ,x.rminusγ,x.hop,x.r,x.hopp,x.hopm,x.eps,x.Dirac_operator,x.MaxCGstep,x.BoundaryCondition)
    end

    function substitute_fermion!(H,j,x::WilsonFermion)
        i = 0
        for ialpha = 1:4
            for it=1:x.NT
                for iz=1:x.NZ
                    for iy=1:x.NY
                        for ix=1:x.NX
                            @simd for ic=1:x.NC
                                i += 1
                                H[i,j] = x[ic,ix,iy,iz,it,ialpha]
                            end
                        end
                    end
                end
            end
        end
    end

    function Wx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1}) where  {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        set_wing_fermi!(x)
        for ν=1:4
            fermion_shift!(temp1,U,ν,x)

            #... Dirac multiplication
            mul!(temp1,view(x.rminusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x)
            mul!(temp2,view(x.rplusγ,:,:,ν),temp2)

            add!(temp,x.hopp[ν],temp1,x.hopm[ν],temp2)

        end

        clear!(xout)
        add!(xout,1,x,-1,temp)

        #display(xout)
        #    exit()
        return
    end

    function Wx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},fparam::FermiActionParam_Wilson) where  {T <: FermionFields,G <: GaugeFields}
        Wx!(xout,U,x,temps)
        return
    end

    function Wx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},fparam::FermiActionParam_WilsonClover) where  {T <: FermionFields,G <: GaugeFields}
        Wx!(xout,U,x,temps,fparam.CloverFμν)
        return
    end

    function Wx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},CloverFμν::AbstractArray) where  {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        set_wing_fermi!(x)
        for ν=1:4
            fermion_shift!(temp1,U,ν,x)

            #... Dirac multiplication
            mul!(temp1,view(x.rminusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x)
            mul!(temp2,view(x.rplusγ,:,:,ν),temp2)

            add!(temp,x.hopp[ν],temp1,x.hopm[ν],temp2)

        end

        clear!(xout)
        add!(xout,1,x,-1,temp)

        cloverterm!(xout,CloverFμν,x)
        #println( "xout ",xout*xout)

        #display(xout)
        #    exit()
        return
    end

    function Wdagx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},CloverFμν::AbstractArray) where {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        x5 = temps[3]

        mul_γ5x!(x5,x)
        set_wing_fermi!(x5)

        for ν=1:4
            fermion_shift!(temp1,U,ν,x5)

            #... Dirac multiplication
            mul!(temp1,view(x.rminusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x5)

            mul!(temp2,view(x.rplusγ,:,:,ν),temp2)

            add!(temp,x.hopp[ν],temp1,x.hopm[ν],temp2)
        end
        clear!(temp1)
        add!(temp1,1,x5,-1,temp)

        cloverterm!(temp1,CloverFμν,x5)

        mul_γ5x!(xout,temp1)
        return
    end

    function Wdagx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},fparam::FermiActionParam_WilsonClover) where {T <: FermionFields,G <: GaugeFields}
        Wdagx!(xout,U,x,temps,fparam.CloverFμν)
        return

    end

    function Wdagx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1}) where  {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        set_wing_fermi!(x)
        for ν=1:4
            fermion_shift!(temp1,U,ν,x)

            #... Dirac multiplication
            #mul!(temp1,view(x.rminusγ,:,:,ν),temp1)
            mul!(temp1,view(x.rplusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x)
            #mul!(temp2,view(x.rplusγ,:,:,ν),temp2)
            mul!(temp2,view(x.rminusγ,:,:,ν),temp2)

            add!(temp,x.hopp[ν],temp1,x.hopm[ν],temp2)

        end

        clear!(xout)
        add!(xout,1,x,-1,temp)

        #display(xout)
        #    exit()
        return
    end

    function Wdagx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1},fparam::FermiActionParam_Wilson) where {T <: FermionFields,G <: GaugeFields}
        Wdagx!(xout,U,x,temps)
        return
    end

    function Dx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1}) where  {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        set_wing_fermi!(x)
        for ν=1:4
            fermion_shift!(temp1,U,ν,x)

            #... Dirac multiplication
            mul!(temp1,view(x.rminusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x)
            mul!(temp2,view(x.rplusγ,:,:,ν),temp2)

            add!(temp,0.5,temp1,0.5,temp2)

        end

        clear!(xout)
        add!(xout,1/(2*x.hop),x,-1,temp)

        #display(xout)
        #    exit()
        return
    end

    function Ddagx!(xout::WilsonFermion,U::Array{G,1},
        x::WilsonFermion,temps::Array{T,1}) where  {T <: FermionFields,G <: GaugeFields}
        temp = temps[4]
        temp1 = temps[1]
        temp2 = temps[2]

        clear!(temp)
        set_wing_fermi!(x)
        for ν=1:4
            fermion_shift!(temp1,U,ν,x)

            #... Dirac multiplication
            #mul!(temp1,view(x.rminusγ,:,:,ν),temp1)
            mul!(temp1,view(x.rplusγ,:,:,ν),temp1)

            #
            fermion_shift!(temp2,U,-ν,x)
            #mul!(temp2,view(x.rplusγ,:,:,ν),temp2)
            mul!(temp2,view(x.rminusγ,:,:,ν),temp2)

            add!(temp,0.5,temp1,0.5,temp2)

        end

        clear!(xout)
        add!(xout,1/(2*x.hop),x,-1,temp)

        #display(xout)
        #    exit()
        return
    end

    function LinearAlgebra.mul!(xout::WilsonFermion,A::AbstractMatrix,x::WilsonFermion)
        NX = x.NX
        NY = x.NY
        NZ = x.NZ
        NT = x.NT
        NC = x.NC

        #n6 = size(x.f)[6]
        #f = zeros(ComplexF64,4)
        #e = zeros(ComplexF64,4)

        for ic=1:NC
            for it=1:NT
                for iz=1:NZ
                    for iy=1:NY
                        @simd for ix=1:NX
                                e1 = x[ic,ix,iy,iz,it,1]
                                e2 = x[ic,ix,iy,iz,it,2]
                                e3 = x[ic,ix,iy,iz,it,3]
                                e4 = x[ic,ix,iy,iz,it,4]

                                xout[ic,ix,iy,iz,it,1] = A[1,1]*e1+A[1,2]*e2+A[1,3]*e3+A[1,4]*e4
                                xout[ic,ix,iy,iz,it,2] = A[2,1]*e1+A[2,2]*e2+A[2,3]*e3+A[2,4]*e4
                                xout[ic,ix,iy,iz,it,3] = A[3,1]*e1+A[3,2]*e2+A[3,3]*e3+A[3,4]*e4
                                xout[ic,ix,iy,iz,it,4] = A[4,1]*e1+A[4,2]*e2+A[4,3]*e3+A[4,4]*e4

                                #=
                            for k1=1:4
                                e[k1] = x[ic,ix,iy,iz,it,k1]
                            end

                            for k1=1:4
                                f[k1] = 0
                                for k2=1:4
                                    f[k1] += A[k1,k2]*e[k2]
                                end
                                xout[ic,ix,iy,iz,it,k1] = f[k1]
                            end
                            =#

                        end
                    end
                end
            end
        end
    end

    function LinearAlgebra.mul!(xout::WilsonFermion,x::WilsonFermion,A::AbstractMatrix)
        NX = x.NX
        NY = x.NY
        NZ = x.NZ
        NT = x.NT
        NC = x.NC

        #n6 = size(x.f)[6]
        #f = zeros(ComplexF64,n6)
        #e = zeros(ComplexF64,n6)

        for ic=1:NC
            for it=1:NT
                for iz=1:NZ
                    for iy=1:NY
                        @simd for ix=1:NX
                            e1 = x[ic,ix,iy,iz,it,1]
                            e2 = x[ic,ix,iy,iz,it,2]
                            e3 = x[ic,ix,iy,iz,it,3]
                            e4 = x[ic,ix,iy,iz,it,4]

                            xout[ic,ix,iy,iz,it,1] = A[1,1]*e1+A[2,1]*e2+A[3,1]*e3+A[4,1]*e4
                            xout[ic,ix,iy,iz,it,2] = A[1,2]*e1+A[2,2]*e2+A[3,2]*e3+A[4,2]*e4
                            xout[ic,ix,iy,iz,it,3] = A[1,3]*e1+A[2,3]*e2+A[3,3]*e3+A[4,3]*e4
                            xout[ic,ix,iy,iz,it,4] = A[1,4]*e1+A[2,4]*e2+A[3,4]*e3+A[4,4]*e4

                            #=
                            for k1=1:n6
                                e[k1] = x[ic,ix,iy,iz,it,k1]
                            end

                            for k1=1:n6
                                f[k1] = 0
                                for k2=1:n6
                                    f[k1] += e[k2]*A[k2,k1]
                                end
                                xout[ic,ix,iy,iz,it,k1] = f[k1]
                            end
                            =#

                        end
                    end
                end
            end
        end
    end

    function fermion_shift!(b::WilsonFermion,u::Array{T,1},μ::Int,a::WilsonFermion) where T <: SU3GaugeFields
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = 3#a.NC

        #NTrange = get_looprange(NT)
        #println(NTrange)
        if μ > 0
            #idel = zeros(Int64,4)
            #idel[μ] = 1

            n6 = size(a.f)[6]
            for ialpha=1:4
                #for it=NTrange
                for it=1:NT
                    it1 = it + ifelse(μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz + ifelse(μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(μ ==2,1,0) #idel[2]
                            @simd for ix=1:NX
                                ix1 = ix + ifelse(μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = u[μ][1,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][1,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][1,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = u[μ][2,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][2,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][2,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[3,ix,iy,iz,it,ialpha] = u[μ][3,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][3,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][3,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                #=
                                for k1=1:3
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:3
                                        b[k1,ix,iy,iz,it,ialpha] += u[μ][k1,k2,ix,iy,iz,it]*a[k2,ix1,iy1,iz1,it1,ialpha]
                                    end
                                end
                                =#
                            end
                        end
                    end
                end
            end

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it - ifelse(-μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz - ifelse(-μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy - ifelse(-μ ==2,1,0)  #idel[2]
                            @simd for ix=1:NX
                                ix1 = ix - ifelse(-μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(u[-μ][1,1,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,1,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,1,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = conj(u[-μ][1,2,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,2,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,2,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[3,ix,iy,iz,it,ialpha] = conj(u[-μ][1,3,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,3,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,3,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]
                                #=
                                for k1=1:3
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:3
                                        b[k1,ix,iy,iz,it,ialpha] += conj(u[-μ][k2,k1,ix1,iy1,iz1,it1])*a[k2,ix1,iy1,iz1,it1,ialpha]
                                    end
                                end
                                =#
                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shift_gamma!(b::WilsonFermion,u::Array{T,1},μ::Int,a::WilsonFermion) where T <: SU3GaugeFields
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = 3#a.NC

        #NTrange = get_looprange(NT)
        #println(NTrange)
        if μ > 0
            #idel = zeros(Int64,4)
            #idel[μ] = 1

            n6 = size(a.f)[6]
            for ialpha=1:4
                #for it=NTrange
                for it=1:NT
                    it1 = it + ifelse(μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz + ifelse(μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(μ ==2,1,0) #idel[2]
                            @simd for ix=1:NX
                                ix1 = ix + ifelse(μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = u[μ][1,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][1,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][1,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = u[μ][2,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][2,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][2,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[3,ix,iy,iz,it,ialpha] = u[μ][3,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][3,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][3,3,ix,iy,iz,it]*a[3,ix1,iy1,iz1,it1,ialpha]

                                #=
                                for k1=1:3
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:3
                                        b[k1,ix,iy,iz,it,ialpha] += u[μ][k1,k2,ix,iy,iz,it]*a[k2,ix1,iy1,iz1,it1,ialpha]
                                    end
                                end
                                =#
                            end
                        end
                    end
                end
            end

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it - ifelse(-μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz - ifelse(-μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy - ifelse(-μ ==2,1,0)  #idel[2]
                            @simd for ix=1:NX
                                ix1 = ix - ifelse(-μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(u[-μ][1,1,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,1,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,1,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = conj(u[-μ][1,2,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,2,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,2,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]

                                b[3,ix,iy,iz,it,ialpha] = conj(u[-μ][1,3,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,3,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][3,3,ix1,iy1,iz1,it1])*a[3,ix1,iy1,iz1,it1,ialpha]
                                #=
                                for k1=1:3
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:3
                                        b[k1,ix,iy,iz,it,ialpha] += conj(u[-μ][k2,k1,ix1,iy1,iz1,it1])*a[k2,ix1,iy1,iz1,it1,ialpha]
                                    end
                                end
                                =#
                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shift!(b::WilsonFermion,u::Array{T,1},μ::Int,a::WilsonFermion) where T <: SU2GaugeFields
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = a.NC

        #NTrange = get_looprange(NT)
        #println(NTrange)
        if μ > 0
            #idel = zeros(Int64,4)
            #idel[μ] = 1

            n6 = size(a.f)[6]
            for ialpha=1:4
                #for it=NTrange
                for it=1:NT
                    it1 = it + ifelse(μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz + ifelse(μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(μ ==2,1,0) #idel[2]
                            for ix=1:NX
                                ix1 = ix + ifelse(μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = u[μ][1,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][1,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = u[μ][2,1,ix,iy,iz,it]*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            u[μ][2,2,ix,iy,iz,it]*a[2,ix1,iy1,iz1,it1,ialpha]

                                #=
                                for k1=1:3
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:3
                                        b[k1,ix,iy,iz,it,ialpha] += u[μ][k1,k2,ix,iy,iz,it]*a[k2,ix1,iy1,iz1,it1,ialpha]
                                    end
                                end
                                =#
                            end
                        end
                    end
                end
            end

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it - ifelse(-μ ==4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz - ifelse(-μ ==3,1,0) #idel[3]
                        for iy=1:NY
                            iy1 = iy - ifelse(-μ ==2,1,0)  #idel[2]
                            for ix=1:NX
                                ix1 = ix - ifelse(-μ ==1,1,0) #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(u[-μ][1,1,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,1,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha]

                                b[2,ix,iy,iz,it,ialpha] = conj(u[-μ][1,2,ix1,iy1,iz1,it1])*a[1,ix1,iy1,iz1,it1,ialpha] +
                                                            conj(u[-μ][2,2,ix1,iy1,iz1,it1])*a[2,ix1,iy1,iz1,it1,ialpha]

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,u::Array{GaugeFields{SU{NC}},1},μ,a::WilsonFermion) where NC
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        #NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            for ix=1:NX
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                for k1=1:NC
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:NC
                                        b[k1,ix,iy,iz,it,ialpha] += conj(a[k2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][k1,k2,ix,iy,iz,it])
                                    end
                                end

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,evensite,u::Array{GaugeFields{SU{NC}},1},μ,a::WilsonFermion)  where NC
        if μ == 0
            substitute!(b,a)
            return
        end

        ibush = ifelse(evensite,0,1)

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        #NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            xran =1+(1+ibush+iy+iz+it)%2:2:NX
                            for ix in xran
                            #for ix=1:NX
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                for k1=1:NC
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:NC
                                        b[k1,ix,iy,iz,it,ialpha] += conj(a[k2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][k1,k2,ix,iy,iz,it])
                                    end
                                end

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,u::Array{SU3GaugeFields,1},μ,a::WilsonFermion)
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            for ix=1:NX
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])* conj(u[-μ][1,3,ix,iy,iz,it])

                                b[2,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,1,ix,iy,iz,it])+
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,3,ix,iy,iz,it])

                                b[3,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,3,ix,iy,iz,it])

                                #=
                                for k1=1:NC
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:NC
                                        b[k1,ix,iy,iz,it,ialpha] += conj(a[k2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][k1,k2,ix,iy,iz,it])
                                    end
                                end
                                =#

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,evensite,u::Array{SU3GaugeFields,1},μ,a::WilsonFermion)
        if μ == 0
            substitute!(b,a)
            return
        end

        ibush = ifelse(evensite,0,1)

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            xran =1+(1+ibush+iy+iz+it)%2:2:NX
                            for ix in xran
                            #for ix=1:NX
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])* conj(u[-μ][1,3,ix,iy,iz,it])

                                b[2,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,1,ix,iy,iz,it])+
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,3,ix,iy,iz,it])

                                b[3,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,2,ix,iy,iz,it]) +
                                                            conj(a[3,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][3,3,ix,iy,iz,it])

                                #=
                                for k1=1:NC
                                    b[k1,ix,iy,iz,it,ialpha] = 0
                                    for k2=1:NC
                                        b[k1,ix,iy,iz,it,ialpha] += conj(a[k2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][k1,k2,ix,iy,iz,it])
                                    end
                                end
                                =#

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,u::Array{SU2GaugeFields,1},μ,a::WilsonFermion)
        if μ == 0
            substitute!(b,a)
            return
        end

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            for ix=1:NX
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,2,ix,iy,iz,it])

                                b[2,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,1,ix,iy,iz,it])+
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,2,ix,iy,iz,it])

                            end
                        end
                    end
                end
            end
        end

    end

    function fermion_shiftB!(b::WilsonFermion,evensite,u::Array{SU2GaugeFields,1},μ,a::WilsonFermion)
        if μ == 0
            substitute!(b,a)
            return
        end

        ibush = ifelse(evensite,0,1)

        NX = a.NX
        NY = a.NY
        NZ = a.NZ
        NT = a.NT
        NC = a.NC

        if μ > 0
            error("""
            Sorry this case if not yet ready
            mu = $mu
            """)

        elseif μ < 0
            #idel = zeros(Int64,4)
            #idel[-μ] = 1
            #n6 = size(b.f)[6]
            for ialpha =1:4
                for it=1:NT
                    it1 = it + ifelse(-μ == 4,1,0) #idel[4]
                    for iz=1:NZ
                        iz1 = iz +ifelse(-μ == 3,1,0)  #idel[3]
                        for iy=1:NY
                            iy1 = iy + ifelse(-μ == 2,1,0) #idel[2]
                            xran =1+(1+ibush+iy+iz+it)%2:2:NX
                            for ix in xran
                                ix1 = ix + ifelse(-μ == 1,1,0)  #idel[1]

                                b[1,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,1,ix,iy,iz,it]) +
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][1,2,ix,iy,iz,it])

                                b[2,ix,iy,iz,it,ialpha] = conj(a[1,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,1,ix,iy,iz,it])+
                                                            conj(a[2,ix1,iy1,iz1,it1,ialpha])*conj(u[-μ][2,2,ix,iy,iz,it])

                            end
                        end
                    end
                end
            end
        end

    end

    """
    c--------------------------------------------------------------------------c
    c     y = gamma_5 * x
    c     here
    c                  ( -1       )
    c        GAMMA5 =  (   -1     )
    c                  (     +1   )
    c                  (       +1 )
    c--------------------------------------------------------------------------c
        """
        function mul_γ5x!(y::WilsonFermion,x::WilsonFermion)
            NX = x.NX
            NY = x.NY
            NZ = x.NZ
            NT = x.NT
            NC = x.NC
            for ic=1:NC
                for it=1:NT
                    for iz=1:NZ
                        for iy=1:NY
                            @simd for ix=1:NX
                                y[ic,ix,iy,iz,it,1] = -1*x[ic,ix,iy,iz,it,1]
                                y[ic,ix,iy,iz,it,2] = -1*x[ic,ix,iy,iz,it,2]
                                y[ic,ix,iy,iz,it,3] = x[ic,ix,iy,iz,it,3]
                                y[ic,ix,iy,iz,it,4] = x[ic,ix,iy,iz,it,4]
                            end
                        end
                    end
                end
            end
        end

        function mul_1plusγ5x!(y::WilsonFermion,x::WilsonFermion) #(1+gamma_5)/2
            NX = x.NX
            NY = x.NY
            NZ = x.NZ
            NT = x.NT
            NC = x.NC
            for ic=1:NC
                for it=1:NT
                    for iz=1:NZ
                        for iy=1:NY
                            @simd for ix=1:NX
                                y[ic,ix,iy,iz,it,1] = 0#-1*x[ic,ix,iy,iz,it,1]
                                y[ic,ix,iy,iz,it,2] = 0#-1*x[ic,ix,iy,iz,it,2]
                                y[ic,ix,iy,iz,it,3] = x[ic,ix,iy,iz,it,3]
                                y[ic,ix,iy,iz,it,4] = x[ic,ix,iy,iz,it,4]
                            end
                        end
                    end
                end
            end
        end

        function mul_1plusγ5x_add!(y::WilsonFermion,x::WilsonFermion,factor) #x = x +(1+gamma_5)/2
            NX = x.NX
            NY = x.NY
            NZ = x.NZ
            NT = x.NT
            NC = x.NC
            for ic=1:NC
                for it=1:NT
                    for iz=1:NZ
                        for iy=1:NY
                            @simd for ix=1:NX
                                y[ic,ix,iy,iz,it,3] += factor*x[ic,ix,iy,iz,it,3]
                                y[ic,ix,iy,iz,it,4] += factor*x[ic,ix,iy,iz,it,4]
                            end
                        end
                    end
                end
            end
        end

        function mul_1minusγ5x!(y::WilsonFermion,x::WilsonFermion) #(1-gamma_5)/2
            NX = x.NX
            NY = x.NY
            NZ = x.NZ
            NT = x.NT
            NC = x.NC
            for ic=1:NC
                for it=1:NT
                    for iz=1:NZ
                        for iy=1:NY
                            @simd for ix=1:NX
                                y[ic,ix,iy,iz,it,1] = x[ic,ix,iy,iz,it,1]
                                y[ic,ix,iy,iz,it,2] = x[ic,ix,iy,iz,it,2]
                                y[ic,ix,iy,iz,it,3] = 0#x[ic,ix,iy,iz,it,3]
                                y[ic,ix,iy,iz,it,4] = 0#x[ic,ix,iy,iz,it,4]
                            end
                        end
                    end
                end
            end
        end

        function mul_1minusγ5x_add!(y::WilsonFermion,x::WilsonFermion,factor) #+(1-gamma_5)/2
            NX = x.NX
            NY = x.NY
            NZ = x.NZ
            NT = x.NT
            NC = x.NC
            for ic=1:NC
                for it=1:NT
                    for iz=1:NZ
                        for iy=1:NY
                            @simd for ix=1:NX
                                y[ic,ix,iy,iz,it,1] += factor*x[ic,ix,iy,iz,it,1]
                                y[ic,ix,iy,iz,it,2] += factor*x[ic,ix,iy,iz,it,2]
                            end
                        end
                    end
                end
            end
        end

    """
    mk_gamma()
    c----------------------------------------------------------------------c
c     Make gamma matrix
c----------------------------------------------------------------------c
C     THE CONVENTION OF THE GAMMA MATRIX HERE
C     ( EUCLIDEAN CHIRAL REPRESENTATION )
C
C               (       -i )              (       -1 )
C     GAMMA1 =  (     -i   )     GAMMA2 = (     +1   )
C               (   +i     )              (   +1     )
C               ( +i       )              ( -1       )
C
C               (     -i   )              (     -1   )
C     GAMMA3 =  (       +i )     GAMMA4 = (       -1 )
C               ( +i       )              ( -1       )
C               (   -i     )              (   -1     )
C
C               ( -1       )
C     GAMMA5 =  (   -1     )
C               (     +1   )
C               (       +1 )
C
C     ( GAMMA_MU, GAMMA_NU ) = 2*DEL_MU,NU   FOR MU,NU=1,2,3,4
c----------------------------------------------------------------------c
    """
    function mk_gamma(r)
        g0 = zeros(ComplexF64,4,4)
        g1 = zero(g0)
        g2 = zero(g1)
        g3 = zero(g1)
        g4 = zero(g1)
        g5 = zero(g1)
        gamma = zeros(ComplexF64,4,4,5)
        rpg = zero(gamma)
        rmg = zero(gamma)

        g0[1,1]=1.0; g0[1,2]=0.0; g0[1,3]=0.0; g0[1,4]=0.0
        g0[2,1]=0.0; g0[2,2]=1.0; g0[2,3]=0.0; g0[2,4]=0.0
        g0[3,1]=0.0; g0[3,2]=0.0; g0[3,3]=1.0; g0[3,4]=0.0
        g0[4,1]=0.0; g0[4,2]=0.0; g0[4,3]=0.0; g0[4,4]=1.0

        g1[1,1]=0.0; g1[1,2]=0.0; g1[1,3]=0.0; g1[1,4]=-im
        g1[2,1]=0.0; g1[2,2]=0.0; g1[2,3]=-im;  g1[2,4]=0.0
        g1[3,1]=0.0; g1[3,2]=+im;  g1[3,3]=0.0; g1[3,4]=0.0
        g1[4,1]=+im;  g1[4,2]=0.0; g1[4,3]=0.0; g1[4,4]=0.0

        g2[1,1]=0.0; g2[1,2]=0.0; g2[1,3]=0.0; g2[1,4]=-1.0
        g2[2,1]=0.0; g2[2,2]=0.0; g2[2,3]=1.0; g2[2,4]=0.0
        g2[3,1]=0.0; g2[3,2]=1.0; g2[3,3]=0.0; g2[3,4]=0.0
        g2[4,1]=-1.0;g2[4,2]=0.0; g2[4,3]=0.0; g2[4,4]=0.0

        g3[1,1]=0.0; g3[1,2]=0.0; g3[1,3]=-im;  g3[1,4]=0.0
        g3[2,1]=0.0; g3[2,2]=0.0; g3[2,3]=0.0; g3[2,4]=+im
        g3[3,1]=+im;  g3[3,2]=0.0; g3[3,3]=0.0; g3[3,4]=0.0
        g3[4,1]=0.0; g3[4,2]=-im;  g3[4,3]=0.0; g3[4,4]=0.0

        g4[1,1]=0.0; g4[1,2]=0.0; g4[1,3]=-1.0;g4[1,4]=0.0
        g4[2,1]=0.0; g4[2,2]=0.0; g4[2,3]=0.0; g4[2,4]=-1.0
        g4[3,1]=-1.0;g4[3,2]=0.0; g4[3,3]=0.0; g4[3,4]=0.0
        g4[4,1]=0.0; g4[4,2]=-1.0;g4[4,3]=0.0; g4[4,4]=0.0

        g5[1,1]=-1.0;g5[1,2]=0.0; g5[1,3]=0.0; g5[1,4]=0.0
        g5[2,1]=0.0; g5[2,2]=-1.0;g5[2,3]=0.0; g5[2,4]=0.0
        g5[3,1]=0.0; g5[3,2]=0.0; g5[3,3]=1.0; g5[3,4]=0.0
        g5[4,1]=0.0; g5[4,2]=0.0; g5[4,3]=0.0; g5[4,4]=1.0

        gamma[:,:,1] = g1[:,:]
        gamma[:,:,2] = g2[:,:]
        gamma[:,:,3] = g3[:,:]
        gamma[:,:,4] = g4[:,:]
        gamma[:,:,5] = g5[:,:]

        for mu=1:4
            for j=1:4
                for i=1:4
                    rpg[i,j,mu] = r*g0[i,j] + gamma[i,j,mu]
                    rmg[i,j,mu] = r*g0[i,j] - gamma[i,j,mu]
                end
            end
        end

        return gamma,rpg,rmg

    end

    function cloverterm!(vec,CloverFμν,x)
        NT = x.NT
        NZ = x.NZ
        NY = x.NY
        NX = x.NX
        NC = x.NC

        i  =0
        for it=1:NT
            for iz=1:NZ
                for iy=1:NY
                    for ix=1:NX
                        i += 1
                        for k1=1:NC
                            for k2=1:NC

                                c1 = x[k2,ix,iy,iz,it,1]
                                c2 = x[k2,ix,iy,iz,it,2]
                                c3 = x[k2,ix,iy,iz,it,3]
                                c4 = x[k2,ix,iy,iz,it,4]

                                vec[k1,ix,iy,iz,it,1] += CloverFμν[k1,k2,i,1]*(-   c1) +
                                                            + CloverFμν[k1,k2,i,2]*(-im*c2) +
                                                            + CloverFμν[k1,k2,i,3]*(-   c2) +
                                                            + CloverFμν[k1,k2,i,4]*(-   c2) +
                                                            + CloverFμν[k1,k2,i,5]*( im*c2) +
                                                            + CloverFμν[k1,k2,i,6]*(-   c1)

                                vec[k1,ix,iy,iz,it,2] += CloverFμν[k1,k2,i,1]*(   c2) +
                                                            + CloverFμν[k1,k2,i,2]*(im*c1) +
                                                            + CloverFμν[k1,k2,i,3]*(-   c1) +
                                                            + CloverFμν[k1,k2,i,4]*(-   c1) +
                                                            + CloverFμν[k1,k2,i,5]*(-im*c1) +
                                                            + CloverFμν[k1,k2,i,6]*(   c2)

                                vec[k1,ix,iy,iz,it,3] += CloverFμν[k1,k2,i,1]*(   -c3) +
                                                            + CloverFμν[k1,k2,i,2]*(-im*c4) +
                                                            + CloverFμν[k1,k2,i,3]*(   c4) +
                                                            + CloverFμν[k1,k2,i,4]*(-   c4) +
                                                            + CloverFμν[k1,k2,i,5]*(-im*c4) +
                                                            + CloverFμν[k1,k2,i,6]*(   c3)

                                vec[k1,ix,iy,iz,it,4] += CloverFμν[k1,k2,i,1]*(   c4) +
                                                            + CloverFμν[k1,k2,i,2]*(im*c3) +
                                                            + CloverFμν[k1,k2,i,3]*(   c3) +
                                                            + CloverFμν[k1,k2,i,4]*(-   c3) +
                                                            + CloverFμν[k1,k2,i,5]*(im*c3) +
                                                            + CloverFμν[k1,k2,i,6]*( -  c4)

                            end
                        end
                    end
                end

            end
        end

        #println("vec = ",vec*vec)

    end

end


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

using Markdown
using InteractiveUtils

# ╔═╡ 2b9eb9cf-d6cc-4dae-bb01-06ae7b62e888
using Plots, PlutoTest

# ╔═╡ 76f31fd4-7025-4134-ae82-34c742cd641d
plotly()

# ╔═╡ f59b7cd9-989e-4911-ab48-6994091b1a74
md"""
# [Day 1: Not Quite Lisp](https://adventofcode.com/2015/day/1)
Santa was hoping for a white Christmas, but his weather machine's "snow" function is powered by stars, and he's fresh out! To save Christmas, he needs you to collect fifty stars by December 25th.

Collect stars by helping Santa solve puzzles. Two puzzles will be made available on each day in the Advent calendar; the second puzzle is unlocked when you complete the first. Each puzzle grants one star. Good luck!

Here's an easy puzzle to warm you up.

Santa is trying to deliver presents in a large apartment building, but he can't find the right floor - the directions he got are a little confusing. He starts on the ground floor (floor `0`) and then follows the instructions one character at a time.

An opening parenthesis, `(`, means he should go up one floor, and a closing parenthesis, `)`, means he should go down one floor.

The apartment building is very tall, and the basement is very deep; he will never find the top or bottom floors.

For example:

-   `(())` and `()()` both result in floor `0`.
-   `(((` and `(()(()(` both result in floor `3`.
-   `))(((((` also results in floor `3`.
-   `())` and `))(` both result in floor `-1` (the first basement level).
-   `)))` and `)())())` both result in floor `-3`.

**To what floor do the instructions take Santa?**
"""

# ╔═╡ 014cabcd-e65d-42ab-b2ce-ec42d233c5e7
puzzle_input = "(((())))()((((((((())()(()))(()((((()(()(((()((()((()(()()()()()))(((()(()((((((((((())(()()((())()(((())))()(()(()((()(()))(()()()()((()((()(((()()(((((((()()())()((((()()(((((()(())()(())((())()()))()(((((((())(()())(()(((())(()))((())))(()((()())))()())((((())))(()(((((()(())(((()()((()((()((((((((((())(()())))))()))())()()((((()()()()()()((((((())())(((()())()((()()(((()()()))(((((()))(((()(()()()(()(()(((())()))(()(((()((())()(()())())))((()()()(()()(((()))(((()((((()(((((()()(()())((()())())(()((((((()(()()))((((()))))())((())()()((()(()))))((((((((()))(()()(((())())(())()((()()()()((()((()((()()(((())))(()((())()((((((((()((()(()()(((())())())))(())())))()((((()))))))())))()()))()())((()())()((()()()))(()()(((()(())((((())())((((((((()()()()())))()()()((((()()))))))()((((()(((()))(()()())))((()()(((()))()()())())(((())((()()(())()()()(((())))))()())((()))()))((())()()())()())()()(()))())))())()))(())((()(())))(()(())(()))))(()(())())(()(())(()(()))))((()())()))()((((()()))))())))()()())((())()((()()()))()(((()(()))))(())()()))(((()())))))))))(((())))()))())()))))()()(((())))))))()(()()(()))((()))))((())))((()((())))())))()()(()))())()(()((()())(()(()()())())(()()))()))))(()())()()))()()()()))(()(()(()))))))()(()))()))()()(()((())(()(())))()(((())(())())))))()(()(()))))()))(()()()(())()(()(())))()))))()()(((((())))))())()())())())()())()))))()))))))))())()()()()()()())))()))((())()))())))()((())()))))()))())))))))())()()()))()()(()((((()(((((((()(())((()())((()()))()))))(())))()()()(())((())()())))(())))(())))(((()()))()(())(((()(()))((())))())()))((((()))())()))))))))()(())())))(()))()(()()))())()()(())())))())()()(()())))()((()())(()(())(())))))))))))))(()))))()))))))()()())(()(((((()(()())))())()))(()))()))(()()))()())(()))())()(())((()()))))))())))())()(((())))(()(()))()()))()(()))))))((()())(()))))))()())))()()))))))))((((((((()()()(()))))))()())))())))()()((())()))((())(())))())())))()()()((()((()(())))())()(())))))))))()())))()()()()()()))()))((())())(()(()))))))(()()))()))(())))()))))))))))))(()))))))))()))))()))()())()))()()))))))()))))((()))))(()))())()(())))(()())((((()())))()))))(()))()(()()(())))))())))))()))))))())))())))))())))())())))())(()))))(())()(())))())()))((()()))))))())))((())))))))())))(())))))()()())))))())))))()))))))()))()()()(()(((()())())())(()))())))))((()(())(()))))))))(())))()()()())())(()))))()()()))()))())())())()(())))()(((()((((())))))))()))))))))))))))))))))((())()())(()))))()()))))))(()()(())())))())))((())))((())))))))))))))()))))()(()))))))())))))()))(()()())(()())))))))))()))))))(())))))()()))()())(((())))()))(()))))))))(())())))())))())())())()()))((())()(())()())()))()())(())(()))))()())))(()(((()))))))()(()())()()()))()))))))))()()()(())()())()(((((()))()())())(()))))()()()(())))())))()((()())))(()))())()(()())())(()))()()))((()()))((()()()()())))(())()))(()(())))((()()))))))))())))))))())()()))))))))))))))))(())()(())(())()())())()))()(()))))())())))))()())()(()))()()(())))(())())))))(()))))))))))))))())())(())(())))(((()))()))))())((())(()))())))))))())))))())))()))()))))))))))))())()))))()))))((()))(())))()(())))(())()))()))())))())))))))()(()())())))()()())))(())))))(()))))))))))))(()))()))()))())))(((()()()(())((()())))()())(((()))(())()))((()()()())))())(())(()))))()(((((())))(()))())())))))))((((()()()))())())()(()(()())))))))))()())())))(())))()())(((()(())())()()))())())))))))((()())((()()(()))(()(())))()))()))(()))(()))()()(()(((())((((()))()(()))((())()(()(()())()(()))()())))))(()))()))())()())))())))(())))((())(()())))))()))(())(()))()())()(()()((()(()))))))()(())(()())(())()))(((())()))(()()(()()()))))(()(())))()))))())))))())(()()()()()()(((())))(()()))()((())(((((()()())))(()))(()))()()))(((())())()(((()()()()))))(()))(())())))()())(()()())())))))))()))))((())))()())(()))(()(()))())))))())(())))))()()())())()))()()(())))(()))(())((((((())(()))(()))())()))(()()(())))()))(()()))()))()(())))(())))((()(()))(())()()())())))(((()()())(())()))))))()(((()(((((()()(((())(())))())()((()))))((()())()(())(((())))(((()((()(()(()))(()()))())(()))(())(())))()))))))((((()))()((((()(()))()))()()))))()(()(()))()(()((()(((()(()()(((()))))()(((()(()(()(((()(()())())()()(()(()())())(()((((())(()))()))(((((()()())(())()((()()())))()()(((()()))()((((((((()(())))())((()))))(())))(()))))((()((((()()(())(((((()))(((((((((((((()())))((((()(((()((())())()))((()))()(()()((()()()()(()()(()(()(((())()(()((((((()((()()((())()((((()((()()(()()())((()()()((()((())()(()(((()((())((((())(()))((()(()))(()())()((((((((()(((((((((((()))(()(((()(()()()((((())((())()())()))(())((())(()))(((()((()(())))(()))))((()()))))((((()(()(()())(()(())((((((((()((((()((()(((((()))())()(()))(()()((()(())(((((()(())()(((((()()))))))()(((())()(()()((((())()((())((()(((())(((()))((()()((((()(())))))((()((((()((()((()(((())((()))(((((((()(((()((((((((())()))((((())(((((()((((((((()(((()((()(((()()(((()((((((()()(()((((((((()()(()(()(())((((()())()))))(((()))((((())((((()())((()(())()((()((((((()((((((()(())))()())(((())())())()(())()(()())((()()((((())((((((())(()(((((()((((())()((((()(()(())(()())(((())()((())((((()))()((((((())(()(((()(((()((((((()(((()))(()()())())((()((()())()((((())(((()(()(((((((((())(())))()((()()()()(())((()))(((((((()(((((((((()(()))))(()((((((((()((((()((()()((((((()()(((((((()(()(())()(())((()()()((()(((((()())()(((((()())()()((()(()())(()()()(((()()(((((()((((((()()((()(()()()((((((((((((()((((((((()()(((()())))()(((()()(())())((((()((((()((((()()()(())(())((()(()(((((((((((((((()(())(())))))()()))((()(((()(())((()(((()(()()((((()()(((()(((()(((((()()((()(()(((()))((((((()((((((((()((()((())(((((()(((())(())())((()()))((((())()()((()(((()(((((()()(((()))(((()(()(((((((((((((()))((((((((()(((()))))())((((((((((((())((())((()())(((())((())(()((((((((((()(((())((()()(()((())(((((((((((()))((((((((((((()(()())((()((()((()(()(((()((((((((()()(()((()(()(((()))((()))(((((((((((((()(())((((((())(((()(())(()(()(()((()()))((((()((((()((((())))())((((()((((()))((((((()((((((()((()(((())))((())(()))(()((()((((()((()(((()()))((((()()()(((((((())(((())(()))())((((()())(((()(((((((((((()(()(()((()(((((((((((((((()()((((()((((((((()(((()()((()((((()))(((()(())((((((()((((())()((((()((()))(())()(()(((()((())())((((((()(()(())())(((())(()(()())(((((()((()((())()())(())))(((()(())))))))(((()(((()))()((()(((()()((()())()()))())))(((()))(()(((()(((((((((()(()(((((()()(((()())()()))))()(((()))(((()(()(()(()(()))()(())()))(()(((())))(()))))))))))(())((()((())((()(())()(())((()()((((()()((()()))((())(((()((()(())(())))()(()(((((()((()))())()(((((()()(((()(()((((((())(()))(())()))((()(()()))(())())()))(((())))(()((()(((())(())())))((()()((((((((((((((()((()(()()(()(((()))())()()((()()()(())(()))(()())(((())((())()(())()()(()()(())))((()(((()))))(((()()(()()))())((()((())()))((((()()()())((())))(((()(())(((((()(((((()((()(()((((()()(((()()()(((()())(((()()((((())(()))(((()))(())())((()))(((()((()))(((()()((())((()(((((()((((()()())((()))()((((()((()(()()()("

# ╔═╡ febdc2a5-b736-4c4d-81f7-bccd7c68d725
read_movements(movements::AbstractString) =
	map(x -> x == '(' ? 1 : -1, collect(movements))

# ╔═╡ 6fee2f51-f7a7-46a9-a54d-33552394e525
@assert sum(read_movements("(())")) == 0

# ╔═╡ 6bad88a8-168a-4710-bc8f-7953d5779165
@assert sum(read_movements("()()")) == 0

# ╔═╡ 6e36a546-357e-483f-bd2d-d87bb97edddb
@assert sum(read_movements("(((")) == 3

# ╔═╡ 4d6bd12a-d7b0-4084-affa-38cbb3554928
@assert sum(read_movements("(()(()(")) == 3

# ╔═╡ d42a1ec4-6ddb-476e-a259-54175b7d8a6e
@assert sum(read_movements("))(((((")) == 3

# ╔═╡ f23b09a3-b95c-4d72-8195-1cc36acce0cf
@assert sum(read_movements("())")) == -1

# ╔═╡ f4e781f0-6331-4750-8908-8be6f6645a9d
@assert sum(read_movements("))(")) == -1

# ╔═╡ 54a7cd1f-e4de-4064-a6f4-fd09f4e5730c
@assert sum(read_movements(")))")) == -3

# ╔═╡ aa539a5f-6140-4bc6-bfac-2ba208508e04
@assert sum(read_movements(")())())")) == -3

# ╔═╡ 0d0133fc-e7f2-4776-8a87-0ab75fc06f70
puzzle_movements = read_movements(puzzle_input)

# ╔═╡ 647d7f09-2ba8-40e0-8e78-168099e5e8b1
part1 = sum(puzzle_movements)

# ╔═╡ e2a0da86-9869-48cc-922f-8f4248127d8e
@test part1 == 232

# ╔═╡ c039b088-9f32-4381-8128-21f5ffb9d5b4
md"Your puzzle answer was `232`."

# ╔═╡ af2306cd-fa6e-495c-b427-f6c0bc38e6ee
md"""
# Part Two
Now, given the same instructions, find the position of the first character that causes him to enter the basement (floor `-1`). The first character in the instructions has position `1`, the second character has position `2`, and so on.

For example:

-   `)` causes him to enter the basement at character position `1`.
-   `()())` causes him to enter the basement at character position `5`.

**What is the position of the character that causes Santa to first enter the basement?**
"""

# ╔═╡ eabd9935-40c5-4ae2-9873-12163d35723f
begin
	plot(cumsum(puzzle_movements), label = false)
	hline!([-1], label="basement")
end

# ╔═╡ 3aabdaf4-3654-486f-91ac-01f5f2db3c87
function below_zero_index(movements::Vector)::Int
	cumsum = 0
	for (idx, val) in enumerate(movements)
		cumsum += val
		if cumsum < 0
			return idx
		end
	end
end

# ╔═╡ 759a6fbf-8875-42b7-a657-63054c61da3e
@assert below_zero_index(read_movements(")")) == 1

# ╔═╡ 2368ed8d-bae9-4878-bcdb-85277c8b26d3
@assert below_zero_index(read_movements("()())")) == 5

# ╔═╡ 16580bf5-0eaa-4023-b315-7ac09d3d81b1
part2 = below_zero_index(puzzle_movements)

# ╔═╡ ec77d418-36d3-438e-b0a1-e28b3f8be4eb
md"Your puzzle answer was `1783`."

# ╔═╡ 08daf9ff-6d3c-4dd2-b499-dda090425e5a
@test part2 == 1783

# ╔═╡ Cell order:
# ╠═2b9eb9cf-d6cc-4dae-bb01-06ae7b62e888
# ╠═76f31fd4-7025-4134-ae82-34c742cd641d
# ╟─f59b7cd9-989e-4911-ab48-6994091b1a74
# ╟─014cabcd-e65d-42ab-b2ce-ec42d233c5e7
# ╠═febdc2a5-b736-4c4d-81f7-bccd7c68d725
# ╠═6fee2f51-f7a7-46a9-a54d-33552394e525
# ╠═6bad88a8-168a-4710-bc8f-7953d5779165
# ╠═6e36a546-357e-483f-bd2d-d87bb97edddb
# ╠═4d6bd12a-d7b0-4084-affa-38cbb3554928
# ╠═d42a1ec4-6ddb-476e-a259-54175b7d8a6e
# ╠═f23b09a3-b95c-4d72-8195-1cc36acce0cf
# ╠═f4e781f0-6331-4750-8908-8be6f6645a9d
# ╠═54a7cd1f-e4de-4064-a6f4-fd09f4e5730c
# ╠═aa539a5f-6140-4bc6-bfac-2ba208508e04
# ╠═0d0133fc-e7f2-4776-8a87-0ab75fc06f70
# ╠═647d7f09-2ba8-40e0-8e78-168099e5e8b1
# ╠═e2a0da86-9869-48cc-922f-8f4248127d8e
# ╟─c039b088-9f32-4381-8128-21f5ffb9d5b4
# ╟─af2306cd-fa6e-495c-b427-f6c0bc38e6ee
# ╠═eabd9935-40c5-4ae2-9873-12163d35723f
# ╠═3aabdaf4-3654-486f-91ac-01f5f2db3c87
# ╠═759a6fbf-8875-42b7-a657-63054c61da3e
# ╠═2368ed8d-bae9-4878-bcdb-85277c8b26d3
# ╠═16580bf5-0eaa-4023-b315-7ac09d3d81b1
# ╟─ec77d418-36d3-438e-b0a1-e28b3f8be4eb
# ╠═08daf9ff-6d3c-4dd2-b499-dda090425e5a

ext: jl


text:
using LinearAlgebra
###################################################

"""
    get_image_xy(camera_params::Camera_Params, params::Video_Params, object::Coordinate)

Returns the object's position on the 2D image in pixel space.

(0,0) is the center of the image
"""
function get_image_xy(camera_params::Camera_Params, params::Video_Params, object::Coordinate)
    #for left-handed coordinate system like TDW, flip the sign of the z-coordinate
    object = Coordinate(object.x, object.y, -object.z)
    camera_loc = Coordinate(camera_params.camera_location.x, camera_params.camera_location.y, -camera_params.camera_location.z)
    camera_look = Coordinate(camera_params.camera_focus.x, camera_params.camera_focus.y, -camera_params.camera_focus.z)
    camera_params = Camera_Params(camera_loc, camera_look)

    if on_right_side(camera_params, object) > 0
        x, y = locate(camera_params, params, object)
    else #on wrong side of camera
        x = Inf
        y = Inf
    end
    return x, y
end

"""
    on_right_side(camera_params::Camera_Params, object::Coordinate)

Checks if the object is on the right side of camera.

The object is on the right side of the camera if
``\\vec{(\\mathrm{focus} - \\mathrm{camera})} \\cdot \\vec{(\\mathrm{object} - \\mathrm{camera})} > 0``
"""
function on_right_side(camera_params::Camera_Params, object::Coordinate)
    c = camera_params.camera_location
    f = camera_params.camera_focus
    (f.x - c.x)*(object.x - c.x) + (f.y - c.y)*(object.y - c.y) + (f.z - c.z)*(object.z - c.z)
end

"""
    locate(camera_params::Camera_Params, params::Video_Params, object::Coordinate)

Given camera info and object's location, finds object's location on 2-D image
"""
function locate(camera_params::Camera_Params,
    params::Video_Params, object::Coordinate)

    (a_vertical, b_vertical, c_vertical) = get_vertical_plane(camera_params)
    (a_horizontal, b_horizontal, c_horizontal) = get_horizontal_plane(camera_params, a_vertical, b_vertical, c_vertical)

    #verified working to here.
    #println("here")
    #println("a,b,c vertical ", (a_vertical, b_vertical, c_vertical))

    s_x = object.x-camera_params.camera_location.x
    s_y = object.y-camera_params.camera_location.y
    s_z = object.z-camera_params.camera_location.z

    # object = [object.x, object.y, object.z]
    # camera_focus = [camera_params.camera_focus.x, camera_params.camera_focus.y, camera_params.camera_focus.z]
    # camera_loc = [camera_params.camera_location.x, camera_params.camera_location.y, camera_params.camera_location.z]
    #
    # println("dot ", dot((camera_focus - camera_loc), (object - camera_focus)))

    (s_x_v, s_y_v, s_z_v) = proj_vec_to_plane(a_vertical, b_vertical, c_vertical, s_x, s_y, s_z)
    (s_x_h, s_y_h, s_z_h) = proj_vec_to_plane(a_horizontal, b_horizontal, c_horizontal, s_x, s_y, s_z)

    #println("s_x_v, s_y_v, s_z_v horizontal ", (s_x_v, s_y_v, s_z_v))
    #println("s_x_h, s_y_h, s_z_h horizontal ", (s_x_h, s_y_h, s_z_h))

    angle_from_vertical = get_angle(a_vertical, b_vertical, c_vertical, s_x_h, s_y_h, s_z_h)
    angle_from_horizontal = get_angle(a_horizontal, b_horizontal, c_horizontal, s_x_v, s_y_v, s_z_v)

    #angle_from_vertical = get_angle(a_vertical, b_vertical, c_vertical, s_x, s_y, s_z)
    #angle_from_horizontal = get_angle(a_horizontal, b_horizontal, c_horizontal, s_x, s_y, s_z)

    #println("angle_from_vertical ", rad2deg(angle_from_vertical))
    #println("angle_from_horizontal ", rad2deg(angle_from_horizontal))

    #sin won't differentiate between angles above and below 90 degrees. So 60 degrees and 120 will look the same, but should be okay since can
    #only be between +90 and -90 since on right side of the camera.

    x = params.image_dim_x/2 * (angle_from_vertical / deg2rad(params.horizontal_FoV/2))
    y = params.image_dim_y/2 * (-angle_from_horizontal / deg2rad(params.vertical_FoV/2))

    x = x + params.image_dim_x/2
    y = y + params.image_dim_y/2

    return (x, y)
end

"""
    get_angle(a, b, c, x, y, z)

Returns the angle between the vector ``(x, y, z)`` and the plane ``(a, b, c)``

Whether the sin is positive or negative depends on the normal.
"""
function get_angle(a, b, c, x, y, z)
    numerator = a*x + b*y + c*z #took out abs
    denominator = sqrt(a^2+b^2+c^2) * sqrt(x^2+y^2+z^2)
    val = numerator/denominator #sometimes got -1.0000000000000002
    val = minimum([val, 1.0])
    val = maximum([val, -1.0])
    return asin(numerator/denominator) #angle in radians
end

"""
    get_vertical_plane(camera_params::Camera_Params)

Returns coefficients a,b,c for the vertically-oriented plane which includes
the vector between the camera focus and position.

Note that the y-axis is the upward one.
"""
function get_vertical_plane(camera_params::Camera_Params)
    p1 = camera_params.camera_location
    p2 = camera_params.camera_focus
    p3 = Coordinate(p1.x, p1.y+1, p1.z)
    (a, b, c) = get_abc_plane(p1, p2, p3)
    #want normal to be be on the "righthand" side of camera
    #x and z component of vec for the direction camera is pointing
    camera_pointing_x = camera_params.camera_focus.x-camera_params.camera_location.x
    camera_pointing_y = camera_params.camera_focus.y-camera_params.camera_location.y
    camera_pointing_z = camera_params.camera_focus.z-camera_params.camera_location.z

    result = cross([a, b, c], [camera_pointing_x, camera_pointing_y, camera_pointing_z])
    #if y-component < 0, multiply by -1
    to_return = result[2] < 0 ? (-a, -b, -c) : (a, b, c)
    return(to_return)
end

"""
    get_horizontal_plane(camera_params::Camera_Params, a_vertical, b_vertical, c_vertical)

Returns coefficients a, b, c for the horizontally oriented plane that includes
the vector between the camera focus and position

This implementation depends on already having determined the vertically-oriented plane.
Note that our implementation returns a placeholder value when the camera is
pointing upwards.
"""
function get_horizontal_plane(camera_params::Camera_Params, a_vertical, b_vertical, c_vertical)
    p1 = camera_params.camera_location
    p2 = camera_params.camera_focus
    p3 = Coordinate(p1.x+a_vertical, p1.y+b_vertical, p1.z+c_vertical) #adding normal vector (a, b, c) to the point to make a third point on the horizontal plane
    (a, b, c) = get_abc_plane(p1, p2, p3)
    #make sure that this normal is upright, so b > 0
    if b < 0
        return(-a, -b, -c)
    elseif b > 0
        return(a, b, c)
    else #when b==0, camera is looking straight up or straight down.
        println("uh oh. camera looking straight up or straight down")
        return(a, b, c)
    end
end

"""
    get_abc_plane(p1::Coordinate, p2::Coordinate, p3::Coordinate)

Given 3 points on a plane (p1, p2, p3), get coefficients for the plane
determined by the 3 points.
"""
function get_abc_plane(p1::Coordinate, p2::Coordinate, p3::Coordinate)
    #using Method 2 from wikipedia: https://en.wikipedia.org/wiki/Plane_(geometry)#:~:text=In%20mathematics%2C%20a%20plane%20is,)%20and%20three%2Ddimensional%20space.
    D = det([p1.x p2.x p3.x; p1.y p2.y p3.y; p1.z p2.z p3.z])

    if D==0
        println("crap! determinant D=0")
        println(p1, p2, p3)
    end
    #implicitly going to say d=-1 to obtain solution set
    a = det([1 1 1; p1.y p2.y p3.y; p1.z p2.z p3.z])/D
    b = det([p1.x p2.x p3.x; 1 1 1; p1.z p2.z p3.z])/D
    c = det([p1.x p2.x p3.x; p1.y p2.y p3.y; 1 1 1])/D

    #normalize / make unit normal vectors
    #a = a / sqrt(a^2 + b^2 + c^2)
    #b = b / sqrt(a^2 + b^2 + c^2)
    #c = c / sqrt(a^2 + b^2 + c^2)
    return (a, b, c)
end

"""
    proj_vec_to_plane(a, b, c, x, y , z)

Projects a vector onto a plane.
"""
function proj_vec_to_plane(a, b, c, x, y , z)
    num = a * x + b * y + z * c
    denom = a^2 + b^2 + c^2
    constant = num/denom
    u_1 = x - constant * a
    u_2 = y - constant * b
    u_3 = z - constant * c
    return (u_1, u_2, u_3)
end

export get_image_xy

max_stars_repo_name: zhangir-azerbayev/MetaGen

max_issues_repo_name: zhangir-azerbayev/MetaGen

max_forks_repo_name: zhangir-azerbayev/MetaGen


text:
#
# benchmarks.jl --
#
# Perform a few benchmark tests on OptimPack arrays.
#
# ----------------------------------------------------------------------------
#
# This file is part of OptimPack.jl which is licensed under the MIT "Expat"
# License:
#
# Copyright (C) 2014-2019, Éric Thiébaut.
#
# ----------------------------------------------------------------------------

module OptimPackBenchmarks

using OptimPack
using BenchmarkTools

function slowvnorm2(x::AbstractArray{T,N}) where {T<:AbstractFloat,N}
    res = zero(T)
    for i in eachindex(x)
        res += x[i]^2
    end
    return sqrt(res)
end

function slowvdot(x::AbstractArray{T,N},
                  y::AbstractArray{T,N}) where {T<:AbstractFloat,N}
    @assert axes(x) == axes(y)
    res = zero(T)
    for i in eachindex(x, y)
        res += x[i]*y[i]
    end
    return res
end

function slowvcopy!(dst::AbstractArray{T,N},
                    src::AbstractArray{T,N}) where {T,N}
    @assert axes(dst) == axes(src)
    for i in eachindex(dst, src)
        dst[i] = src[i]
    end
    return dst
end

function vnorm2(x::AbstractArray{T,N}) where {T<:AbstractFloat,N}
    res = zero(T)
    @inbounds @simd for i in eachindex(x)
        res += x[i]^2
    end
    return sqrt(res)
end

function vdot(x::AbstractArray{T,N},
              y::AbstractArray{T,N}) where {T<:AbstractFloat,N}
    @assert axes(x) == axes(y)
    res = zero(T)
    @inbounds @simd for i in eachindex(x, y)
        res += x[i]*y[i]
    end
    return res
end

function vcopy!(dst::AbstractArray{T,N},
                src::AbstractArray{T,N}) where {T,N}
    @assert axes(dst) == axes(src)
    @inbounds @simd for i in eachindex(dst, src)
        dst[i] = src[i]
    end
    return dst
end

T = Float64
dims = (20,30,40)
len = prod(dims)

x = randn(T, dims)
y = randn(T, dims)
z = randn(T, dims)

E = OptimPack.DenseVariableSpace(T, dims)
vx = vcopy!(OptimPack.create(E), x)
vy = vcopy!(OptimPack.create(E), y)
vz = vcopy!(OptimPack.create(E), z)

@assert vnorm2(vz) == vnorm2(z)
@assert vnorm2(vz) ≈ OptimPack.norm2(vz)
@assert vdot(vx,vy) == vdot(x,y)
@assert vdot(vx,vy) ≈ OptimPack.dot(vx,vy)

println("\nSlow versions (with bound checking):")
println("  VNORM2 of $len elements:")
print("    Julia arrays ------------->")
@btime slowvnorm2($z)
print("    DenseVariable arrays ----->")
@btime slowvnorm2($vz)
println("  VDOT of $len elements:")
print("    Julia arrays ------------->")
@btime slowvdot($x,$y)
print("    DenseVariable arrays ----->")
@btime slowvdot($vx,$vy)
print("    mixed type arrays -------->")
@btime slowvdot($x,$vy)
print("    mixed type arrays -------->")
@btime slowvdot($vx,$y)

println("\nFast versions (no bound checking and SIMD):")
println("  VNORM2 of $len elements:")
print("    Julia arrays ------------->")
@btime vnorm2($z)
print("    DenseVariable arrays ----->")
@btime vnorm2($vz)
println("  VDOT of $len elements:")
print("    Julia arrays ------------->")
@btime vdot($x,$y)
print("    DenseVariable arrays ----->")
@btime vdot($vx,$vy)
print("    mixed type arrays -------->")
@btime vdot($x,$vy)
print("    mixed type arrays -------->")
@btime vdot($vx,$y)

println("\nVersions provided by the C library:")
print("  VNORM2 of $len elements ---->")
@btime OptimPack.norm2($vz)
print("  VDOT of $len elements ------>")
@btime OptimPack.dot($vx,$vy)

end


text:
using ImageBase, OffsetArrays, StackViews
using ImageFiltering
using Test, TestImages, Aqua, Documenter

using OffsetArrays: IdentityUnitRange
include("testutils.jl")

@testset "ImageBase.jl" begin
    @testset "Project meta quality checks" begin
        if VERSION >= v"1.3"
            # Not checking compat section for test-only dependencies
            Aqua.test_ambiguities(ImageBase)
            Aqua.test_all(ImageBase;
                ambiguities=false,
                project_extras=true,
                deps_compat=true,
                stale_deps=true,
                project_toml_formatting=true
            )
            doctest(ImageBase, manual = false)
        end
    end

    include("diff.jl")
    include("restrict.jl")
    include("statistics.jl")

    @info "deprecations are expected"
    include("deprecated.jl")
end


text:
"""
    Lon <: XDim <: Dimension
    Lon(val=:)

Longitude [`Dimension`]($DDdimdocs).

## Example:
```julia
longdim = Lon(10:10:100)
# Or
val = A[Lon(1)]
# Or
mean(A; dims=Lon)
```
"""
@dim Lon XDim "Longitude"

"""
    Lat <: YDim <: Dimension
    Lat(val=:)

Latitude [`Dimension`]($DDdimdocs).

## Example:
```julia
vertdim = Lat(10:10:100)
# Or
val = A[Lat(1)]
# Or
mean(A; dims=Lat)
```
"""
@dim Lat YDim "Latitude"

"""
    Vert <: ZDim <: Dimension
    Vert(val=:)

Vertical [`Dimension`]($DDdimdocs).

## Example:
```julia
vertdim = Vert(10:10:100)
# Or
val = A[Vert(1)]
# Or
mean(A; dims=Vert)
```
"""
@dim Vert ZDim "Vertical"

"""
    Band <: Dimension
    Band(val=:)

Band [`Dimension`]($DDdimdocs) for multi-band rasters.

## Example:
```julia
banddim = Band(10:10:100)
# Or
val = A[Band(1)]
# Or
mean(A; dims=Band)
```
"""
@dim Band

"""
    userbounds(x)

Get the bounds converted to the `usercrs` value.

Whithout ArchGDAL loaded, this is just the regular bounds.
"""
function userbounds end

userbounds(A) = userbounds(dims(A))
userbounds(dims::Tuple) = map(userbounds, dims)
userbounds(dim::Dimension) = bounds(dim)

"""
    userval(x)

Get the index value of a dimension converted to the `usercrs` value.

Whithout ArchGDAL loaded, this is just the regular dim value.
"""
function userval end

userval(A) = userval(dims(A))
userval(dims::Tuple) = map(userval, dims)
userval(dim::Dimension) = val(dim)


text:
using Robotlib, TotalLeastSquares, FillArrays
using Plots
# using HomotopyContinuation, DynamicPolynomials
using Test, Random
RTR(R) = R'R
vecangle(g, gf) = acos(min(1, g ⋅ gf / norm(gf) / norm(g))) * 180 / pi

rcond(K) = /(svdvals(K)[[1, 3]]...)
toR(r) = orthonormal(reshape(real(r), 3, 3))
tor(r) = toR(r)[:]

# calib_force_iterative is located in the calib_force.jl file

function calib_force_iterative2(POSES, F, g; trace = false)
    N = size(POSES, 3)
    I = Robotlib.I3
    A = Array{eltype(F)}(undef, 3N, 3)
    for i = 1:N
        A[3(i-1)+1:3i, :] = POSES[:, :, i]'
    end
    A = factorize(A)
    local m
    trace && (Rg = [])
    for iter = 1:6
        Rf, m = calib_force(
            POSES,
            F,
            g;
            offset = false,
            verbose = false,
        )
        B = (Rf*F')[:]
        g = A \ B
        trace && push!(Rg, (Rf, g))
    end
    trace && (return Rf, g, m, Rg)
    Rf, g, m
end

function calib_force_iterative3(POSES, F; trace = false)
    N = size(POSES, 3)
    I = Robotlib.I3
    Rf = I
    A = Array{eltype(F)}(undef, 3N, 3)
    B = Array{eltype(F)}(undef, 3N)
    local m, g
    trace && (Rg = [])
    for iter = 1:6
        for i = 1:N
            A[3(i-1)+1:3i, :] = Rf'POSES[:, :, i]'
            B[3(i-1)+1:3i] = F[i, :]
        end
        g = A \ B
        Rf, m = calib_force(
            POSES,
            F,
            g;
            offset = false,
            verbose = false,
        )
        trace && push!(Rg, (Rf, g))
    end
    trace && (return Rf, g, m, Rg)
    Rf, g, m
end

function calib_force_iterative4(POSES, forces; trace = false)
    N = size(POSES, 3)
    I3 = Matrix{Float64}(I, 3, 3)
    r = vec(I3)
    # D = permutedims(POSES, (2,1,3))
    # D = reshape(D, 3N,3)
    D = reshape(POSES, 3, 3N)'
    F = kron(forces, I3)
    FF = pinv(F) * D
    DD = pinv(D) * F
    K = FF * DD
    e = eigen(K)
    sort(e.values, by = abs, rev = true)
    trace && (Rg = [])
    for iter = 1:5
        r = K * r
        r = vec(orthonormal(reshape(r, 3, 3)))
        # trace && push!(Rg, (Rf, g))
    end
    # trace && (return Rf,g,m,Rg)
    # Rf,g,m
    orthonormal(reshape(r, 3, 3)), e
end

##
σ = 1
traces = map(1:30) do mc
    N = 100
    Rf = Robotlib.orthonormal(randn(3, 3))
    gf = 100randn(3)
    POSES =
        cat([Robotlib.orthonormal(randn(3, 3)) for i = 1:N]..., dims = 3)
    forces =
        hcat([Rf' * (POSES[1:3, 1:3, i]' * gf) for i = 1:N]...)' |> copy
    forces .+= σ * randn(size(forces))
    R, g, m, Rg = calib_force_iterative3(POSES, forces, trace = true)
    # R, g = calib_force_eigen(POSES, forces)
    Rf, gf, Rg, R, g
end
##
errors = map(traces) do (Rf, gf, Rg, R, g)
    # R, g = Rg[end]
    Rerr = Rangle(R, Rf, true)
    gerr = norm(g - gf) #< 1e-10 + √3*3σ/sqrt(N)
    Rerr, gerr
end

histogram([getindex.(errors, 1) getindex.(errors, 2)], layout = 2)
##
errortraces = map(traces) do (Rf, gf, Rg)
    Rerr = [Rangle(R, Rf, true) for (R, g) in Rg]
    gerr = [norm(g - gf) / norm(gf) for (R, g) in Rg] #< 1e-10 + √3*3σ/sqrt(N)
    gaerr = [vecangle(g, gf) for (R, g) in Rg]
    Rerr, gerr, gaerr
end

default(size = (800, 600), grid = true, linealpha = 0.3, linecolor = :black)
scales = (yscale = :identity, xscale = :identity, legend = false)
ylims = (ylims = (0, 30),)
plot(
    getindex.(errortraces, 1);
    layout = (1, 3),
    subplot = 1,
    scales...,
    title = "\$R\$ angle [deg]",
)
hline!([180], subplot = 1, l = (:black, :dash))
plot!(
    getindex.(errortraces, 2);
    subplot = 2,
    scales...,
    title = "\$g\$ relative error",
)
plot!(
    getindex.(errortraces, 3);
    subplot = 3,
    scales...,
    title = "\$g\$ angle [deg]",
)

##
opts = (fillalpha = 0.3,)
histogram(
    [e[1][end] for e in errortraces];
    layout = (3, 1),
    subplot = 1,
    title = "Rangle",
    opts...,
)
histogram!(
    [Rangle(t[1], t[4]) * 180 / pi for t in traces];
    subplot = 1,
    opts...,
)

histogram!(
    [e[2][end] for e in errortraces];
    subplot = 2,
    title = "g relative",
    opts...,
)
histogram!(
    [norm(t[2] - t[5]) / norm(t[2]) for t in traces];
    subplot = 2,
    opts...,
)

histogram!(
    [e[3][end] for e in errortraces];
    subplot = 3,
    title = "g angle",
    opts...,
)
histogram!([vecangle(t[2], t[5]) for t in traces]; subplot = 3, opts...)

# hline!([√3*3σ/sqrt(N)], l=(:black,:dash, 3), sp=2)

# using SumOfSquares, PolyJuMP, Ipopt
# m = SOSModel(with_optimizer(Ipopt.Optimizer))

# N         = 1000
# Rf        = Robotlib.orthonormal(randn(3,3))
# mf        = 1
# gf        = [0,0,1] + 0.3randn(3)
# POSES     = cat([Robotlib.orthonormal(randn(3,3)) for i = 1:N]..., dims=3)
# forces    = hcat([Rf'*(POSES[1:3,1:3,i]'*gf) for i = 1:N]...)' |> copy
#
#
# function calib_forceHomotopy(POSES,F,m0=1)
#     N = size(POSES,3)
#     @polyvar w[1:9]
#     @polyvar g[1:3]
#     @polyvar λ[1:6]
#     I = Robotlib.I3
#     A  = Array{eltype(F)}(undef, 3N, 9)
#     B  = Array{Any}(undef, 3N)
#     At = (F,i) -> [F[i,1]*I   F[i,2]*I    F[i,3]*I]
#     for i = 1:N
#         RA               = POSES[1:3,1:3,i]'
#         A[3(i-1)+1:3i,:] = At(F,i)
#         B[3(i-1)+1:3i]   = -RA*g
#     end
#     @info("Condition number of Gram matrix: ", cond(A'A))
#     R = reshape(w,3,3)
#     J = sum((A*w-B).^2) + sum(1:3) do i
#         λ[i]*(R[:,i]'R[:,i] - 1)
#     end + λ[4]*R[:,1]'R[:,2] +
#     λ[5]*R[:,1]'R[:,3] +
#     λ[6]*R[:,2]'R[:,3]
#
#     dJ     = DynamicPolynomials.differentiate(J, [w;g;λ])
#     result = solve(dJ)
#     reals  = realsolutions(result)
#     costfun = w-> begin
#     sum((A*vec(w[1:9])-[b(w[10:12]) for b in B]).^2)
# end
# # R = tomat(reals[minindex])
# result, costfun
# end
#
#
#
# result, J = calib_forceHomotopy(POSES,forces)
# reals = realsolutions(result)
# tomat(x) = reshape(x[1:9],3,3) |> copy
#
# Rs = tomat.(reals)
# RTR(R) = R'R
# minval, minindex = findmin(J.(reals))
# R = Rs[minindex]

max_forks_repo_name: baggepinnen/Robotlib


text:
@testset "Operators" begin
    @testset "Coulomb operator" begin
        k = 7
        N = 31

        a,b = 0,70

        coulomb(r) = -1/r
        @testset "$name knot set" for (name, t, tol) in [("Linear", LinearKnotSet(k, a, b, N), 9e-3),
                                                         ("Exponential", ExpKnotSet(k, -1.0, log10(b), N), 2e-7)]
            B = BSpline(t)[:,2:end-1]
            S = B'B

            r = axes(B,1)

            f = B \ (r -> r^2 * exp(-r)).(r)
            g = B \ (r -> -r*exp(-r)).(r)

            V = B'*QuasiDiagonal(coulomb.(r))*B
            g̃ = S \ V*f

            @test g ≈ g̃ atol=tol
        end
    end
end


text:
#!/usr/bin/env julia

using Test, SafeTestsets

@time begin
@time @safetestset "Dockerfile file creation" begin include("docker_creation_test.jl") end
end


text:
# x goal
shift = [0.25; 0.0; 0.0]
u = 0.65 * [1.0; 0.0; 0.0] ./ norm(ones(3))

# z goal
shift = [0.0; 0.0; 0.25]
u = 0.65 * [0.0; 0.0; 1.0] ./ norm(ones(3))

anim = MeshCat.Animation(convert(Int, floor(1.0 / dt)))
v = env.vis
z = z_sol[1]
u_max = maximum([norm(u) for u in u_sol])
force_vis = ArrowVisualizer(v[:force])
setobject!(force_vis, MeshPhongMaterial(color=orange))
settransform!(force_vis,
    Point(z[1] - u[1] - shift[1], z[2] - u[2] - shift[2], z[3] - u[3] - shift[3]),
    Vec(u[1], u[2], u[3]),
    shaft_radius=0.05,
    max_head_radius=0.1)

z_vis = [[z_sol[1] for t = 1:15]..., z_sol..., [z_sol[end] for t = 1:15]...]
u_vis = [[u_sol[1] for t = 1:15]..., u_sol..., [u_sol[end] for t = 1:15]...]
for t = 1:length(z_vis)
    z = z_vis[t]
    u = (t == length(z_vis) ? 0.5 * u_vis[end] ./ u_max : 0.5 * u_vis[t] ./ u_max)
    MeshCat.atframe(anim, t) do

        settransform!(v[:robot], MeshCat.compose(MeshCat.Translation(z[2], z[1], z[3]), MeshCat.LinearMap(Quaternion(z[6 .+ (1:4)]...))))
        settransform!(force_vis,
            Point(z[2] - u[2] - shift[2], z[1] - u[1] - shift[1], z[3] - u[3] - shift[3]),
            Vec(u[2], u[1], u[3]),
            shaft_radius=0.05,
            max_head_radius=0.1)
        # set_floor!(env.vis, x=0.0, y=2, z=0.02, color=RGBA(0.7,0.7,0.7,1))
    end
end
MeshCat.setanimation!(v, anim)


text:
abstract type AbstractTrajectory{n,m,T} <: AbstractVector{T} end

terminal_control(Z::AbstractTrajectory) = !RobotDynamics.is_terminal(Z[end])
traj_size(Z::AbstractTrajectory{n,m}) where {n,m} = n,m,length(Z)
num_vars(Z::AbstractTrajectory) = num_vars(traj_size(Z)..., terminal_control(Z))
eachcontrol(Z::AbstractTrajectory) = terminal_control(Z) ? Base.OneTo(length(Z)) : Base.OneTo(length(Z)-1)

function num_vars(n::Int, m::Int, N::Int, equal::Bool=false)
    Nu = equal ? N : N-1
    return N*n + Nu*m
end

function Base.copyto!(Z1::AbstractTrajectory, Z2::AbstractTrajectory)
	@assert traj_size(Z1) == traj_size(Z2)
	for k = 1:length(Z1)
		Z1[k] = Z2[k]
	end
	return Z1
end

@inline states(Z::AbstractTrajectory) = state.(Z)
function controls(Z::AbstractTrajectory)
	return [control(Z[k]) for k in eachcontrol(Z) ]
end

function Base.isapprox(Z1::AbstractTrajectory, Z2::AbstractTrajectory)
    all(zs->zs[1] ≈ zs[2], zip(Z1,Z2))
end

function set_dt!(Z::AbstractTrajectory, dt::Real)
    t = Z[1].t
    for z in Z
        z.dt = dt
        z.t = t
        t += dt
    end
    return t
end

"""
    Traj{n,m,T,KP}

A vector of `AbstractKnotPoint`s of type `KP` with state dimension `n`,
control dimension `m`, and value type `T`

Supports iteration and indexing.

# Constructors
    Traj(n, m, dt, N, equal=false)
    Traj(x, u, dt, N, equal=false)
    Traj(X, U, dt, t)
    Traj(X, U, dt)
"""
struct Traj{n,m,T,KP} <: AbstractTrajectory{n,m,T}
	data::Vector{KP}
	function Traj(Z::Vector{<:AbstractKnotPoint{T,n,m}}) where {n,m,T}
		new{n,m,T,eltype(Z)}(Z)
	end
end

# AbstractArray interface
@inline Base.iterate(Z::Traj, k::Int) = iterate(Z.data, k)
Base.IteratorSize(Z::Traj) = Base.HasLength()
Base.IteratorEltype(Z::Traj) = Base.IteratorEltype(Z.data)
@inline Base.eltype(Z::Traj) = eltype(Z.data)
@inline Base.length(Z::Traj) = length(Z.data)
@inline Base.size(Z::Traj) = size(Z.data)
@inline Base.getindex(Z::Traj, i) = Z.data[i]
@inline Base.setindex!(Z::Traj, v, i) = Z.data[i] = v
@inline Base.firstindex(Z::Traj) = 1
@inline Base.lastindex(Z::Traj) = lastindex(Z.data)
Base.IndexStyle(::Traj) = IndexLinear()

Traj(Z::Traj) = Z

function Base.copy(Z::AbstractTrajectory) where {T,N,M}
    Traj([KnotPoint(copy(z.z), copy(z._x), copy(z._u), z.dt, z.t) for z in Z])
end

function Traj(n::Int, m::Int, dt::AbstractFloat, N::Int; equal=false)
    x = NaN*@SVector ones(n)
    u = @SVector zeros(m)
    Traj(x,u,dt,N; equal=equal)
end

function Traj(x::SVector, u::SVector, dt::AbstractFloat, N::Int; equal=false)
    equal ? uN = N : uN = N-1
    Z = [KnotPoint(x,u,dt,(k-1)*dt) for k = 1:uN]
    if !equal
        m = length(u)
        push!(Z, KnotPoint(x,m,(N-1)*dt))
    end
    return Traj(Z)
end

function Traj(X::Vector, U::Vector, dt::Vector, t=cumsum(dt) .- dt[1])
    Z = [KnotPoint(X[k], U[k], dt[k], t[k]) for k = 1:length(U)]
    if length(U) == length(X)-1
        push!(Z, KnotPoint(X[end],length(U[1]),t[end]))
    end
    return Traj(Z)
end

states(Z::Traj, i) = [state(z)[i] for z in Z]

function set_states!(Z::Traj, X)
    for k in eachindex(Z)
		RobotDynamics.set_state!(Z[k], X[k])
    end
end

function set_states!(Z::Traj, X::AbstractMatrix)
    for k in eachindex(Z)
		RobotDynamics.set_state!(Z[k], X[:,k])
    end
end

function set_controls!(Z::AbstractTrajectory, U)
    for k in 1:length(Z)-1
		RobotDynamics.set_control!(Z[k], U[k])
    end
end

function set_controls!(Z::AbstractTrajectory, U::AbstractMatrix)
    for k in 1:length(Z)-1
		RobotDynamics.set_control!(Z[k], U[:,k])
    end
end

function set_controls!(Z::AbstractTrajectory, u::SVector)
    for k in 1:length(Z)-1
		RobotDynamics.set_control!(Z[k], u)
    end
end

function set_times!(Z::AbstractTrajectory, ts)
    for k in eachindex(ts)
        Z[k].t = ts[k]
        k < length(ts) && (Z[k].dt = ts[k+1] - ts[k])
    end
end

function get_times(Z::Traj)
    [z.t for z in Z]
end

function shift_fill!(Z::Traj, n=1)
    N = length(Z)
    isterm = is_terminal(Z[end])
    for k = 1+n:N
        Z[k-n] = copy(Z[k])
    end
    xf = state(Z[N-n])
    uf = control(Z[N-n])
    dt = Z[N-n-1].dt
    for k = N-n:N
        set_state!(Z[k], xf)
        Z[k].t = Z[k-1].t + dt
        if k == N && is_terminal(Z[k])
            Z[k].dt = 0
        else
            set_control!(Z[k], uf)
            Z[k].dt = dt
        end
    end
end

function Base.copyto!(Z::Traj, Z0::Traj)
	@assert length(Z) == length(Z0)
	for k in eachindex(Z)
		copyto!(Z[k].z, Z0[k].z)
	end
end

function Base.copyto!(Z::Union{Vector{<:KnotPoint},Traj{<:Any,<:Any,<:Any,<:KnotPoint}}, Z0::Traj)
	@assert length(Z) == length(Z0)
	for k in eachindex(Z)
		Z[k].z = Z0[k].z
	end
end

#~~~~~~~~~~~~~~~~~~~~~~~~~~ FUNCTIONS ON TRAJECTORIES ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~#

"Evaluate the discrete dynamics for all knot points"
function discrete_dynamics!(::Type{Q}, f, model, Z::Traj) where Q
    for k in eachindex(Z)
        f[k] = RobotDynamics.discrete_dynamics(Q, model, Z[k])
    end
end

@inline state_diff_jacobian!(G, model::AbstractModel, Z::Traj) = nothing
function state_diff_jacobian!(G, model::LieGroupModel, Z::Traj)
	for k in eachindex(Z)
		G[k] .= 0
		state_diff_jacobian!(G[k], model, Z[k])
	end
end

function rollout!(::Type{Q}, model::AbstractModel, Z::AbstractTrajectory, x0=state(Z[1])) where Q <: QuadratureRule
    set_state!(Z[1], x0)
    for k = 2:length(Z)
        RobotDynamics.propagate_dynamics(Q, model, Z[k], Z[k-1])
    end
end


text:
import Printf

function voxelize(cloud, resolution)
    cloud_xyz = round.(cloud[1:min(size(cloud,1), 3),:] ./ resolution) * resolution
    idxs = unique(i -> cloud_xyz[:,i], 1:size(cloud_xyz)[2])
    cloud[:, idxs]
end

const cube_vertices = [
 1.         -1.          1. ;
-1.         -1.          1. ;
-1.         -1.         -1. ;
-1.          1.         -1. ;
-1.          1.          1. ;
 1.          1.          1. ;
 1.          1.         -1. ;
 1.          1.          1. ;
 1.         -1.          1. ;
 1.          1.          1. ;
-1.          1.          1. ;
-1.         -1.          1. ;
-1.         -1.          1. ;
-1.          1.          1. ;
-1.          1.         -1. ;
 1.         -1.         -1. ;
-1.         -1.         -1. ;
-1.          1.         -1. ;
 1.         -1.         -1. ;
 1.          1.         -1. ;
 1.         -1.         -1. ;
 1.         -1.          1. ;
-1.         -1.         -1. ;
 1.          1.         -1.
 ]

const cube_normals = [
 0. -1.  0.;
 0. -1.  0.;
 0. -1.  0.;
 0.  1.  0.;
 0.  1.  0.;
 0.  1.  0.;
 1.  0.  0.;
 1.  0.  0.;
 1.  0.  0.;
-0.  0.  1.;
-0.  0.  1.;
-0.  0.  1.;
-1. -0. -0.;
-1. -0. -0.;
-1. -0. -0.;
 0.  0. -1.;
 0.  0. -1.;
 0.  0. -1.;
 0. -1.  0.;
 0.  1.  0.;
 1.  0.  0.;
-0.  0.  1.;
-1. -0. -0.;
 0.  0. -1.;
]

const cube_faces = [
 0  1  2;
 3  4  5;
 6  7  8;
 9 10 11;
12 13 14;
15 16 17;
18  0  2;
19  3  5;
20  6  8;
21  9 11;
22 12 14;
23 15 17;
]

function mesh_from_voxelized_cloud(cloud, resolution)
    new_v = vcat([cube_vertices .* resolution/2.0 .+ r' for r in eachcol(cloud)]...)
    new_n = vcat([cube_normals for _ in 1:size(cloud)[2]]...)
    new_f = vcat([cube_faces .+ 24*(i-1) for i in 1:size(cloud)[2]]...)
    Mesh(vertices=permutedims(new_v),
         indices=permutedims(new_f), normals=permutedims(new_n))
end

function box_mesh_from_dims(dims)
    Mesh(
        vertices=permutedims(cube_vertices .* (dims ./ 2.0)'),
        indices=permutedims(cube_faces),
        normals=permutedims(cube_normals)
    )
end

function box_wireframe_mesh_from_dims(dims, w)
    x,y,z = dims
    wireframe_mesh = +([
        let
            mesh_x = box_mesh_from_dims([x + w, w, w])
            mesh_x = Pose([0.0, s_x * y/2.0, s_y * z/2.0]) * mesh_x
            mesh_y = box_mesh_from_dims([w, y + w, w])
            mesh_y = Pose([s_x * x/2.0, 0.0, s_y * z/2.0]) * mesh_y
            mesh_z = box_mesh_from_dims([w, w, z + w])
            mesh_z = Pose([s_x * x/2.0, s_y * y/2.0, 0.0]) * mesh_z
            mesh_x + mesh_y + mesh_z
        end
        for (s_x, s_y) in [(-1,1),(-1,-1),(1,-1),(1,1)]
    ]...)
    wireframe_mesh
end

function Base.:(+)(a::Mesh, b::Mesh)::Mesh
    Mesh(
        vertices=hcat(a.vertices, b.vertices),
        indices=hcat(a.indices, size(a.vertices)[2] .+ b.indices),
        normals=hcat(a.normals, b.normals),
    )
end

function Base.:(*)(p::Pose, m::Mesh)::Mesh
    mesh_copy = copy_mesh(m)
    mesh_copy.vertices = p * mesh_copy.vertices
    mesh_copy
end

function get_mesh_data_from_obj_file(obj_file_path; tex_path=nothing, scaling_factor=1.0)
    mesh = FileIO.load(obj_file_path);
    vertices = hcat([[x...] for x in mesh.position]...) .* scaling_factor
    indices = hcat([[map(GB.value,a)...] for a in GB.faces(mesh)]...) .- 1

    if hasproperty(mesh, :normals)
        normals = hcat([[x...] for x in mesh.normals]...)
        normals = Matrix{Float64}(normals)
    else
        normals = nothing
    end

    if hasproperty(mesh, :uv)
        tex_coords = hcat([[x...] for x in mesh.uv]...)
        tex_coords = Matrix{Float64}(tex_coords)
    else
        tex_coords = nothing
    end

    vertices = Matrix{Float64}(vertices)
    indices = Matrix{UInt32}(indices)

    Mesh(vertices=vertices, indices=indices, normals=normals,
         tex_coords=tex_coords, tex_path=tex_path)
end

function write_mesh_to_obj_file(mesh::Mesh, filepath::String)
    open(filepath, "w") do file
        for ver in eachcol(mesh.vertices)
            Printf.@printf(file, "v %.4f %.4f %.4f\n", ver[1],ver[2],ver[3])
        end
        for ver in eachcol(mesh.indices)
            Printf.@printf(file, "f %d %d %d\n", (ver .+ 1)...)
        end
    end
end

export voxelize, mesh_from_voxelized_cloud


text:
export ChannelCreate,
    ChannelUpdate,
    ChannelDelete,
    ChannelPinsUpdate

"""
Sent when a new [`DiscordChannel`](@ref) is created.
"""
struct ChannelCreate <: AbstractEvent
    channel::DiscordChannel
end
@boilerplate ChannelCreate :docs
ChannelCreate(d::Dict{String, Any}) = ChannelCreate(DiscordChannel(d))

"""
Sent when a [`DiscordChannel`](@ref) is updated.
"""
struct ChannelUpdate <: AbstractEvent
    channel::DiscordChannel
end
@boilerplate ChannelUpdate :docs
ChannelUpdate(d::Dict{String, Any}) = ChannelUpdate(DiscordChannel(d))

"""
Sent when a [`DiscordChannel`](@ref) is deleted.
"""
struct ChannelDelete <: AbstractEvent
    channel::DiscordChannel
end
@boilerplate ChannelDelete :docs
ChannelDelete(d::Dict{String, Any}) = ChannelDelete(DiscordChannel(d))

"""
Sent when a [`DiscordChannel`](@ref)'s pins are updated.
"""
struct ChannelPinsUpdate <: AbstractEvent
    channel_id::Snowflake
    last_pin_timestamp::Union{DateTime, Nothing}
end
@boilerplate ChannelPinsUpdate :dict :docs


text:
module TestRank

    using Test
    using NCBITaxonomy

    @test rank(ncbi"Vulpes") == :genus
    @test rank(ncbi"Vulpes vulpes") == :species
    @test parent(ncbi"Vulpes vulpes") == ncbi"Vulpes"

end