text:
# Copyright 2019 Jose Ortiz-Bejar
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

#module Nets
export Net, fft_sampling, kmeans_sampling, density_sampling, random_sampling, gen_features, KMS, f1m
#import KernelMethods.Kernels: sigmoid, gaussian, linear, cauchy, poly, quadratic
using KernelMethods: accuracy, recall#, f1
#import KernelMethods.Supervised: NearNeighborClassifier, optimize!, predict_one, predict_one_proba
using KernelMethods: NearNeighborClassifier, optimize!, predict_one, predict_one_proba
using SimilaritySearch: KnnResult, l2_distance, normalize, squared_l2_distance,  hamming_distance, cosine_distance, angle_distance
using PyCall
import PyCall: PyObject, pyimport
using StatsBase
using Random
using Distributed

#nb=pyimport("sklearn.naive_bayes")
#nn=pyimport("sklearn.neighbors")
#ms=pyimport("sklearn.model_selection")
scipy=pyimport("scipy.stats")
#@pyimport  scipy.stats as scipy
#using DataStructures
# Samplinng Net structure
mutable struct Net
    data::Vector{Vector{Float64}}
    labels::Vector
    #udata::Vector{Vector{Float64}} #unlabeled data
    udata::Vector{Vector{Float64}} #Vector of unlabeled data
    references::Vector{Int32}
    partitions::Vector{Int32}
    centers::Vector#{Vector{Float64}}
    centroids::Vector#{Vector{Float64}}
    dists::Vector{Float64}
    csigmas::Vector{Float64}
    sigmas::Dict{Int,Float64}
    stats::Dict{String,Float64}
    reftype::Symbol
    distance::Symbol
    kernel
    clftype::Symbol #:space or prototypes
    clabels::Vector{Int}
    #sparse::Bool
    #vbows::Vector{BOW}
end

Net(data,labels;udata=[])::Net=Net(data,labels,udata,[],[],[],[],[],[],Dict(),Dict(),
                                    :centroids,:squared_l2_distance,gaussian,:space,[])

##  Kernel Fucntions

function linear(xo,xm,distance;sigma=1)::Float64
    d=distance(xo,xm)
    return d #<=1e-6 ? 1.0 : sigma/d
end

poly(xo,xm,distance;sigma=1,degree=2)=(sigma*distance(xo,xm)+1)^degree

quadratic(xo,xm,distance;sigma=1)=1-distance(xo,xm)^2/(distance(xo,xm)^2+sigma)

maxk(xo,xm,distance;sigma=1) = distance(xo,xm) > sigma ? 1.0 : 0.0

function gaussian(xo,xm,distance; sigma=1)::Float64
    d=distance(xo,xm)
    if d<=1e-6
        return 1.0
    end
    exp(-d^2/(2*sigma^2))
end

sigmoid(xo,xm,distance; sigma=1)=1/(1+exp(distance(xo,xm)-sigma))

function cauchy(xo,xm,distance; sigma=1)
    d=distance(xo,xm)
    if d<=1e-6
        return 1.0
    end
    return 1/(1+d^2/sigma^2)
end

# AUC score
function auc(y,yp)::Float64
    return 2*metrics.roc_auc_score(y, yp)-1
end
# Angle distance of normalize vectors
function angle(a,b)
    an=normalize(a)
    bn=normalize(b)
    angle_distance(an,bn)
end

function f1m(y,yp)::Float64
    metrics=pyimport("sklearn.metrics")
    return metrics.f1_score(y, yp, average="macro")
end

function pearson(y,yp)::Float64
    return scipy.pearsonr(y, yp)[1]
end

function rmse(y,yp)::Float64
    yp=[x>0 ? x : 0 for x in yp]
    aux=log.(yp+1).-log.(y+1)
    sqrt(sum(aux.*aux)/length(y))
end

## Find the next farthest element
function maxmin(data,centers,ind,index,distance,partitions)::Tuple{Int64,Float64}
    c=last(centers)
    if length(index)==0
        for i in ind
            if i!=c
                push!(index,i,Inf)
            end
        end
    end
    nindex=KnnResult(length(index))
    for fn in index
        dist=eval(distance)(data[fn.objID],data[c])
        #push!(lK[fn.objID],dist)
        #@show dist, fn.dist , distance,fn.objID
        #@show data[fn.objID]
        #@show data[c]
        dist = (dist<fn.dist) ?  dist : fn.dist
        partitions[fn.objID] = (dist<fn.dist) ? c : partitions[fn.objID]
        if fn.objID!=c
            if typeof(dist)==Symbol
                @show dist
            end
            push!(nindex,fn.objID,convert(Float64, dist))
        end
    end
    index.k=nindex.k
    index.pool=nindex.pool
    fn=pop!(index)
    return fn.objID,fn.dist
end

## Centroids  using only non zero values
function mean_non_zero(data::Vector{T})::Vector{Float64} where T
    n=length(data[1])
    vmean=Vector{Float64}(undef,n)
    for i in 1:n
        tmp=[x[i] for x in data if x[i]>0]
        vmean[i]= length(tmp)>0 ? mean(tmp) : 0
    end
    vmean
end

function cmean(data)
    n=length(data)
    #if typeof(data[1])==TextSearch.BOW
    #    total=deepcopy(data[1])
    #    for vbow in data[2:end]
    #        total=total+vbow
    #    end
    #    return total
    #else
    return mean(data)
    #end
end

# get a set of Centroids
function get_centroids(N)
    centers=[j for j in Set(N.partitions)]
    sort!(centers)
    #if N.sparse
    #    centroids=Vector{BOW}(length(centers))
    #    data= N.vbows[:]
    #else
    centroids=Vector{Vector{Float64}}(undef,length(centers))
    data = N.data[:]
    #end
    [push!(data,ud) for ud in N.udata];
    for (ic,c) in enumerate(centers)
        ind=[i for (i,v) in enumerate(N.partitions) if v==c]
        #if N.distance!=:cosine
        ##centroids[ic]=mean(N.data[ind])
        centroids[ic]=cmean(data[ind])
        #else
        #centroids[ic]=mean_non_zero(N.data[ind])
        #end
    end
    return centroids
end

function to_column(data)
    X=hcat(data...)
    X=[X[i,:] for i in  1:size(X)[1]]
    return X
end

# Epsilon Network using farthest first traversal Algorithm
function fft_sampling(N,num_of_centers::Int; distance=:squared_l2_distance, axis=1,
    per_class=false,reftype=:centroids, kernel=linear,test_set=false,)
    N.distance=distance
    N.kernel=kernel
    #@show num_of_centers distance  axis N.sparse  reftype per_class Symbol(kernel)
    ##n=length(N.data)
    yc=[]
    #if N.sparse && axis > 0
    #    data=N.vbows
    #    n=length(N.vbows)+length(N.udata)
    #else
    data=N.data
    n=length(N.data)+length(N.udata)
    #end
    ######### Real Inductive ######
    if test_set && length(N.udata)>0 && !per_class
        data=[]
        n=length(N.udata)
    end
    ##############################
    # @show n,N.sparse
    partitions=[0 for i in 1:n]
    gcenters,dists,sigmas=Vector{Int}(undef,0),Vector{Float64}(undef,num_of_centers-1),Dict{Int,Float64}()
    indices=[[i for i in  1:n]]
    #indices=[[i for (i,j) in enumerate(N.labels) if j==l] for l in [1]]
    #@show length(indices)
    L=sort([i for i in Set(N.labels)])
    if per_class
        indices=[[i for (i,j) in enumerate(N.labels) if j==l] for l in L]
        #@show length(indices), "XXXXXXXXXXXXXXXX"
    end
    fdata=data[:]
    if !per_class
        [push!(fdata,ud) for ud in N.udata];
    end
    si=1
    for ind in indices
        centers=Vector{Int}(undef,0)
        s=rand(1:length(ind))
        push!(centers,ind[s])
        index=KnnResult(length(ind))
        partitions[ind[s]]=ind[s]
        k=1
        push!(yc,L[si])
        while  k<=num_of_centers-1 && k<=length(ind)
            ##fnid,d=maxmin(N.data,centers,ind,index,distance,partitions)
            fnid,d=maxmin(fdata,centers,ind,index,distance,partitions)
            push!(centers,fnid)
            dists[k]=d
            partitions[fnid]=fnid
            k+=1
            push!(yc,L[si])
        end
        N.sigmas[L[si]]=minimum(dists)
        si=si+1
        gcenters=vcat(gcenters,centers)
    end
    assign(fdata,fdata[gcenters],partitions;distance=N.distance)
    N.references,N.partitions,N.dists,N.sigmas=gcenters,partitions,dists,sigmas
    ##N.csigmas,N.stats=get_csigmas(N.data,N.centroids,N.partitions,distance=N.distance)
    N.csigmas,N.stats=get_csigmas(fdata,N.centroids,N.partitions,distance=N.distance)
    #N.sigmas[0]=maximum(N.csigmas)
    ##N.centers,N.centroids=N.data[gcenters],get_centroids(N)
    N.centers,N.centroids=fdata[gcenters],get_centroids(N)
    N.reftype=reftype
    N.clabels=yc
end

# KMeans ++ seeding Algorithm

function kmpp(N,num_of_centers::Int)::Vector{Int}
    fdata=N.data[:]
    #if !per_class
    [push!(fdata,ud) for ud in N.udata];
    #end
    n=length(fdata)
    #partitions=[0 for i in 1:n]
    #n=length(N.data)
    s=rand(1:n)
    centers,d=Vector{Int}(undef,num_of_centers),squared_l2_distance
    centers[1]=s
    #D=[d(N.data[j],N.data[s]) for j in 1:n]
    D=[d(fdata[j],fdata[s]) for j in 1:n]
    for i in 1:num_of_centers-1
        cp=cumsum(D/sum(D))
        r=rand()
        sl=[j for j in 1:length(cp) if cp[j]>=r]
        s=sl[1]
        centers[i+1]=s
        for j in 1:n
            dist=d(fdata[j],fdata[s])
            # dist=d(N.data[j],N.data[s])
            if dist<D[j]
                D[j]=dist
            end
        end
    end
    #assign(fdata,fdata[centers],partitions;distance=N.distance)
    centers
end

#Assign Elementes to thier  nearest centroid

function assign(data,centroids,partitions;distance=:squared_l2_distance)
    d=distance
    #@show centroids
    #@show data
    for i in 1:length(data)
        dd=[eval(d)(data[i],c) for c in centroids]
        #@show dd
        partitions[i]=sortperm([eval(d)(data[i],c) for c in centroids])[1]
    end
end

function assignc(data,centroids,partitiontails;distance=:squared_l2_distance)
    d=distance
    for i in 1:length(data)
        partitions[i]=sortperm([eval(d)(data[i],c) for c in centroids])[1]
    end
end

#Distances for each element to its nearest cluster centroid

function get_distances(data,centroids,partitions;distance=:squared_l2_distance)::Vector{Float64}
    dists=Vector{Float64}(undef,length(centroids))
    for i in 1:length(centroids)
        ind=[j for (j,l) in enumerate(partitions) if l==i]
        if length(ind)>0
            X=data[ind]
            dd=[eval(distance)(centroids[i],x) for x in X]
            dists[i]=maximum(dd)
        end
    end
    sort!(dists)
    return dists
end

#Calculated the sigma for each ball

function get_csigmas(data,centroids,partitions;distance=:squared_l2_distance)::Tuple{Vector{Float64},Dict{String,Float64}}
    stats=Dict("SSE"=>0.0,"BSS"=>0.0)
    refs=[j for j in Set(partitions)]
    sort!(refs)
    df=eval(distance)
    if distance==:squared_l2_distance
        df=eval(:l2_distance)
    end
    csigmas=Vector{Float64}(undef,length(refs))
    for (ii,i) in enumerate(refs)
        ind=[j for (j,l) in enumerate(partitions) if l==i]
        #if length(ind)>0
        X=data[ind]
        dd=[df(data[i],x) for x in X]
        csigmas[ii]=max(0,maximum(dd))
        #stats["SSE"]+=sum(dd)
        #stats["BSS"]+=length(X)*(sum(mean(X)-m))^2
        #end
    end
    return csigmas,stats
end

#Feature generator using kmeans centroids

function kmeans_sampling(N,num_of_centers::Int; max_iter=1000,kernel=linear,distance=:squared_l2_distance,reftype=:centroids, per_class=false,test_set=false)
    #n=length(N.data)
    fdata=N.data[:]
    #if !per_class
    [push!(fdata,ud) for ud in N.udata];
    #end
    n=length(fdata)
    #lK,partitions=[[] for i in 1:n],[0 for i in 1:n],[0 for i in 1:n]
    partitions=[0 for i in 1:n]
    dists=Vector{Float64}
    init=kmpp(N,num_of_centers)
    centroids=fdata[init]#N.data[init]
    i,aux=1,Vector{Float64}(undef,length(centroids))
    while centroids != aux && i<max_iter
        i=i+1
        aux = centroids
        assign(fdata,centroids,partitions)
        #assign(N.data,centroids,partitions)
        N.partitions=partitions
        centroids=get_centroids(N)
    end
    N.distance=distance
    dists=get_distances(fdata,centroids,partitions,distance=N.distance)
    N.partitions,N.dists=partitions,dists
    N.centroids,N.sigmas[0]=centroids,maximum(N.dists)
    #N.csigmas,N.stats=get_csigmas(N.data,N.centroids,N.partitions,distance=N.distance)
    N.centers=init
    N.csigmas,N.stats=get_csigmas(fdata,N.centroids,N.partitions,distance=N.distance)
    N.sigmas[0]=maximum(N.csigmas)
    N.reftype=:centroids
    N.kernel=kernel
end

#References selection using naive algoritmh for density net

function density_sampling(N,num_of_elements::Int64; distance=:squared_l2_distance,kernel=linear,reftype=:centroids,per_class=false, test_set=false)
    N.distance=distance
    #if N.sparse
    #    data=N.vbows
    #else
    data=N.data
    #end
    fdata=data[:]
    if !per_class
        [push!(fdata,ud) for ud in N.udata];
    end
    n,d,k=length(fdata),distance,num_of_elements
    k=trunc(Int,n/k)
    partitions,references=[0 for i in 1:n],Vector{Int}(undef,0)
    pk=1
    dists,sigmas=Vector{Float64},Dict{Int,Float64}()
    while 0 in partitions
        pending=[j for (j,v) in enumerate(partitions) if partitions[j]==0]
        s=rand(pending)
        partitions[s]=s
        pending=[j for (j,v) in enumerate(partitions) if partitions[j]==0]
        push!(references,s)
        pc=sortperm([eval(d)(fdata[j],fdata[s]) for j in pending])
        if length(pc)>=k
            partitions[pending[pc[1:k]]]=[s for j in 1:k]
        else
            partitions[pending[pc]]=[s for j in 1:length(pc)]
        end
    end
    N.references,N.partitions=references,partitions
    N.centers,N.centroids=fdata[references],get_centroids(N)
    N.csigmas,N.stats=get_csigmas(fdata,N.centroids,N.partitions,distance=N.distance)
    N.sigmas[0]=maximum(N.csigmas)
    N.reftype=:centroids
    N.distance=distance
    N.kernel=kernel
end

# Select references randomly
function random_sampling(N,num_of_elements::Int64; distance=:squared_l2_distance,kernel=linear,reftype=:centroids, per_class=false,
    test_set=false)
    N.distance=distance
    #if N.sparse
    #    data=N.vbows
    #else
    data=N.data
    #end
    fdata=data[:]
    [push!(fdata,ud) for ud in N.udata];
    n,d,k=length(fdata),distance,num_of_elements
    partitions=[0 for i in 1:length(fdata)]
    pk=1
    dists,sigmas=Vector{Float64},Dict{Int,Float64}()
    references=Random.randperm(n)[1:num_of_elements]
    assign(fdata,fdata[references],partitions,distance=distance)
    N.references,N.partitions=references,partitions
    N.centers,N.centroids=fdata[references],get_centroids(N)
    N.csigmas,N.stats=get_csigmas(fdata,N.centroids,N.partitions,distance=N.distance)
    N.sigmas[0]=maximum(N.csigmas)
    N.reftype=:centroids
    N.distance=distance
    N.kernel=kernel
end

#Find Inverse Homegeinity Index
function IHomogeneity(N)
    G=Set(N.partitions)
    k=length(G)
    h=0.0
    for g in G
        ind=[i for (i,l) in enumerate(N.partitions) if l==g]
        gl=N.labels[ind]
        nc=length(gl)
        mc=first(sort([ v for (k,v) in countmap(gl)],rev=true))
        h+=1/nc*(mc/length(gl))
    end
    k-h
end

#Generates feature espace using cluster centroids or centers
function gen_features(Xo,N)::Vector{Vector{Float64}}
    n=length(Xo)
    sigmas,Xr=N.csigmas,Vector{Vector{Float64}}(undef,n)
    #@show length(N.centers), length(N.centroids)
    Xm = N.reftype==:centroids || length(N.centers)==0 ? N.centroids : N.centers
    nf=length(Xm)
    kernel=N.kernel
    #@show Xm
    #@show N.partitions, N.reftype, N.centers
    for i in 1:n
        xd=Vector{Float64}(undef,nf)
        for j in 1:nf
            #@show i,j,length(Xo),length(Xm),length(sigmas),sigmas
            #@show Xo[i],Xm[j],sigmas[j]
            #xd[j]=kernel(Xo[i],Xm[j],sigma=sigmas[j],distance=eval(N.distance))
            dd=N.distance
            if N.distance==:squared_l2_distance
                dd=:l2_distance
            end
            xd[j]=kernel(Xo[i],Xm[j],eval(dd),sigma=sigmas[j])
        end
        xd[isnan.(xd)].=0.0
        if typeof(xd)==Symbol
            @show "============XXXXXXXXXXXXXXXX======================", xd, i
        end
        Xr[i]=xd
    end
    Xr
end

function KFoldsTrain(N;op_function=recall,folds=3,runs=0,trainratio=0.7,testratio=0.3)
    #data= N.sparse ? N.vbows : N.data
    data=N.data
    X=gen_features(data,N)
    y=N.labels
    ms=pyimport("sklearn.model_selection")
    skf=ms.StratifiedKFold(n_splits=folds)
    skf[:get_n_splits](X,y)
    clfs=genCl()
    opval,cltr,disttr,wtr=-1,"NaiveBayes","ND","NA"
    clfr=clfs[1][1]
    for (clf,clt,distt,wt) in clfs
        y_pred=[]
        yr=[]
        for (ei,vi) in skf[:split](X,y)
            ei,vi=ei+1,vi+1
            xt,xv=X[ei],X[vi]
            yt,yv=y[ei],y[vi]
            #clf[:fit](xt,yt)
            clf.fit(xt,yt)
            #y_pred=clf[:predict](xv)
            yr=vcat(yr,yv)
            #y_pred=vcat(y_pred,clf[:predict](xv)[:,1])
            y_pred=vcat(y_pred,clf.predict(xv)[:,1])
        end
        opv=op_function(yr,y_pred)
        if opv>opval
            clfr,cltr,disttr,wtr=clf,clt,distt,wt
            opval=opv
        end
    end
    #@show  opval, cltr, disttr, N.distance, trainratio,length(N.centroids)
    #clfr[:fit](X,y)
    clfr.fit(X,y)
    return (clfr,opval,cltr,disttr,wtr)
end

function MonteCarloTrain(N;op_function=recall,runs=3,trainratio=0.7,testratio=0.3,folds=0)
    #data= N.sparse ? N.vbows : N.data
    data= N.data
    X=gen_features(data,N)
    y=N.labels
    testratio = testratio>0.0 ? testratio : 1.0-trainratio
    clfs=genCl()
    opval,cltr,disttr,wtr=-1,"MLP","ND","NA"
    clfr=clfs[1][1]
    res=[0 for  clf in clfs]
    for i  in 1:runs
        j=1
        for (clf,clt,distt,wt) in clfs
            xt,xv,yt,yv=ms.train_test_split(X,y,train_size=trainratio,test_size=testratio)
            #clf[:fit](xt,yt)
            clf.fit(xt,yt)
            #y_pred=clf[:predict](xv)[:,1]
            y_pred=clf.predict(xv)[:,1]
            opv=op_function(yv,y_pred)
            res[j]=res[j]+opv
            j+=1
        end
    end
    m=sortperm(res)[end]
    clfr,cltr,disttr,wtr=clfs[m]
    opval=res[m]/runs
    #clfr[:fit](X,y)
    clfr.fit(X,y)
    return (clfr,opval,cltr,disttr,wtr)
end

function predict_test(xt,y,xv,desc,cl)
    y_pred=[]
    nb=pyimport("sklearn.naive_bayes")
    if contains(desc,"KNN")
        #@ typeof(xt), typeof(y), typeof(cl.X.dist), Symbol(cl.X.dist)
        cln=NearNeighborClassifier(xt,y,cl.X.dist,cl.k,cl.weight)
        y_pred=[predict_one_proba(cln,x) for x in xv]
    elseif  contains(desc,"SVC")
        y_tmp=cln[:decision_function](xv)
        y_pred=[[0,1/(1+exp(-yp))] for yp in y_tmp]
    else
        cln=nb.GaussianNB()
        #cln[:fit](xt,y)
        cln.fit(xt,y)
        #y_pred=cln[:predict_proba](xv)
        y_pred=cln.predict_proba(xv)
    end
    y_pred
end

function fisher(N)::Float64
    data,labels=gen_features(N.data,N),N.labels
    classes=Set(labels)
    n=length(data)
    m=length(data[1])
    nc=length(classes)
    cl=Vector{Vector{Int64}}(undef,n)
    for (i,class) in enumerate(classes)
        cl[i]=[j for (j,l) in enumerate(labels) if l==class]
    end
    x1,x2=hcat(data[cl[1]]...)',hcat(data[cl[2]]...)'
    m1,m2=mean(x1,1),mean(x2,1)
    s1,s2=std(x1,1),std(x2,1)
    sum(((m1-m2).^2)/((s1+s2).^2))
end

function genCl()
    K=[1,5,11,21]
    D=["cosine","euclidean"]
    W=["uniform","distance"]
    clfs=[]
    nn=pyimport("sklearn.neighbors")
    nb=pyimport("sklearn.naive_bayes")
    lm=pyimport("sklearn.linear_model")
    svm=pyimport("sklearn.svm")
    for d in D
        for w in W
            for k in K
                if k==1 && w=="distance"
                    continue
                else
                    clf=nn.KNeighborsClassifier(n_neighbors=k,metric=d,weights=w)
                    #@show clf
                    push!(clfs,(clf,"NN$k",d,w))
                end
            end
        end
    end
    #clf_list=[clfs[Random.randperm(length(clfs))][1], (nb.GaussianNB(),"NaiveBayes","ND","NA")]
    clf_list=clfs
    push!(clf_list,(nb.GaussianNB(),"NaiveBayes","ND","NA"))
    #push!(clf_list,(lm.RidgeClassifier(),"Ridge","ND","NA"))
    #push!(clf_list,(lm.LogisticRegression(),"Logistic","ND","NA"))
    #push!(clf_list,(svm.LinearSVC(),"SVM","ND","NA"))
    return clf_list
    #return clf_list[Random.randperm(length(clf_list))][1]
end

function genGrid(nets=[:fft_sampling,:kmeans_sampling,:density_sampling,:random_sampling];K=[4,8,16,32],
    trainings=[:inductive],
    kernels=[:gaussian,:sigmoid,:linear,:cauchy],
    reftypes=[:centers,:centroids],
    distances=[:angle,:squared_l2_distance],
    distancesk=[:angle,:squared_l2_distance],
    #trainratios=[1],
    sample_size=128)
    #clfs=genCl()
    if length(nets)==1 && nets[1]==:kmeans_sampling
        reftypes=[:centroids]
        distancesk=[:squared_l2_distance]
    end
    #space=[(k=k,kernel=kernel,reftype=reftype,distancek=dc,nettype=net,training=training,cl=genCl())
    space=[(k=k,kernel=kernel,reftype=reftype,distancek=dc,nettype=net,training=training,cl=cl)
    for k in K  for kernel in kernels
    for reftype in reftypes for dc in distancesk for net in nets
    for training in trainings
    for cl in genCl()
    if  net!=:kmeans_sampling || reftype!=:centers || (net==:kmeans_sampling && dc==:squared_l2_distance)]
    #if  !(net==:kmeans_sampling  && dc==:angle)]
    #sz = sample_size%2==1 ? trunc(Int,sample_size/2)+1 : trunc(Int,sample_size/2)
    if length(space)>sample_size && sample_size!=-1
        space=space[Random.randperm(length(space))[1:sample_size]]
    end
    space
end

function inductive(Xe,Ye,k,nettype,kernel,distancek,reftype,classifier;
    distances=[:angle,:squared_l2_distance],
    op_function=:recall,folds=3,udata=[], per_class=false,
    test_set=false)
    ms = pyimport("sklearn.model_selection")
    skf=ms.StratifiedKFold(n_splits=folds,shuffle=true)
    skf.get_n_splits(Xe,Ye)
    clf,clt,distt,wt=classifier
    y_pred=[]
    yr=[]
    #@info  "Starting KFOLDS", k,nettype,kernel,distancek,reftype
    for (ei,vi) in skf.split(Xe,Ye)
        ei,vi=ei.+1,vi.+1
        xt,xv=Xe[ei],Xe[vi]
        yt,yv=Ye[ei],Ye[vi]
        N=Net(xt,yt)
        N.data=xt
        N.labels=yt
        eval(nettype)(N,k,kernel=eval(kernel),distance=distancek,reftype=reftype,per_class=per_class)
        Xt=gen_features(xt,N)
        Xv=gen_features(xv,N)
        clf.fit(Xt,yt)
        yr=vcat(yr,yv)
        y_pred=vcat(y_pred,clf.predict(Xv)[:,1])
    end
    #@info  "Finished KFOLDS", k,nettype,kernel,distancek,reftype
    #@info  "Starting Final training =========", k,nettype,kernel,distancek,reftype
    opval=eval(op_function)(yr,y_pred)
    #@info  "Building final NET XXXXXXXXXXXXXXXX"
    Nf=Net(Xe,Ye)
    eval(nettype)(Nf,k,kernel=eval(kernel),distance=distancek,reftype=reftype,per_class=per_class)
    #@info  "final NET finished XXXXXXXXXXXXXXXX"
    #@info  "Generating features for full data"
    X=gen_features(Xe,Nf)
    #@info  "features for full data generated"
    #@info  "Fitting final clasifier"
    clf.fit(X,Ye)
    #@info  "final clasifier fitted =============="
    trainratio=1
    traintype="KFolds"
    key="$nettype/$kernel/$distancek/$k/$clt/$reftype/$trainratio/inductive/$distt/$wt/$traintype"
    return (clf,Nf),(opval,key)
    #return (clfr,opval,cltr,disttr,wtr)
end

function eval_conf(args)
    c,op_function,Xe,Ye,per_class,test_set,folds,udata,debug=args
    #@info length(c)
    #@info  "Configuration Inited", c.k, c.kernel, c.reftype,c.distancek,c.nettype,c.training,length(c.cl)
    #@info op_function,length(Xe),length(Ye),per_class,test_set,folds,udata
    #@info c,op_function,Xe,Ye,per_class,test_set,folds,udata
    (cli,neti),(opvali,ckeyi) = eval(c.training)(Xe,Ye,c.k,c.nettype,c.kernel,c.distancek,c.reftype,
    c.cl; folds=folds,udata=udata, op_function=op_function, per_class=per_class,test_set=test_set)
    #@info "Configuration Evaluated", c.k, c.kernel, c.reftype,c.distancek,c.nettype,c.training,length(c.cl)
    if debug
        @show "#########", opvali, ckeyi
    end
    (cl=cli, net=neti, opval=opvali, ckey=ckeyi)
end

function KMS(Xe,Ye; op_function=:recall,top_k=15,folds=3,per_class=false, udata=[],
    nets=[:fft_sampling,:kmeans_sampling,:density_sampling,:random_sampling],
    K=[4,8,16,32,64],distances=[:angle,:squared_l2_distance],
    distancesk=[:angle,:squared_l2_distance],sample_size=128,
    kernels=[:gaussian,:linear,:cauchy,:sigmoid],test_set=false, debug=false)
    #DNNC=Dict()
    space_temp=genGrid(nets,K=K,kernels=kernels,distancesk=distancesk,sample_size=sample_size,distances=distances)
    space=[(conf,op_function,Xe,Ye,per_class,test_set,folds,udata,debug) for conf in space_temp]
    res=map(eval_conf, space)
    sort!(res, by=x->x.opval, rev=true)
    res[1:top_k]
    #(cli,neti),(opvali,ckeyi)=
    #Top=Vector{Tuple{Float64,String}}(undef, 0)
    #for (k,kernel,reftype,distancek,nettype,training,clfc) in space
        #cln,dkn,cldn=clfc[2],clfc[3],clfc[4]
    #    (cli,neti),(opvali,ckeyi)=eval(training)(Xe,Ye,k,nettype,kernel,distancek,reftype,
    #    clfc; folds=folds,udata=udata, op_function=op_function, per_class=per_class,test_set=test_set)
    #    push!(Top,(opvali,ckeyi))
    #    if debug
    #        @show (debug, opvali,ckeyi)
    #    end
    #    DNNC[ckeyi]=(cli,neti)
    #end
    #for (k,kernel,reftype,distancek,nettype,training) in space
    #    clf_list=genCl()
    #    if sample_size!=-1
    #        clf_list=[clf_list[Random.randperm(length(clf_list))][1]]
    #    end
    #    #push!(clf_list,(nb.GaussianNB(),"NaiveBayes","ND","NA"))
    #    #@show(length(space)*length(clf_list))
    #    cl,net,opval,ckey=nothing,nothing,0,""
    #    clf_list
    #    for clfc in clf_list
    #       cln,dkn,cldn=clfc[2],clfc[3],clfc[4]
    #        (cli,neti),(opvali,ckeyi)=eval(training)(Xe,Ye,k,nettype,kernel,distancek,reftype,clfc;
    #        folds=folds,udata=udata,
    #        op_function=op_function, per_class=per_class,test_set=test_set)
    #        if opvali>opval
    #            (cl,net),(opval,ckey)=(cli,neti),(opvali,ckeyi)
    #        end
    #    end
    #    push!(Top,(opval,ckey))
    #    if debug
    #        @show (debug, opval,ckey)
    #    end
    #    DNNC[ckey]=(cl,net)
    #end
    #sort!(Top,rev=true)
    #Top=Top[1:top_k]
    #[(DNNC[top[2]],top[1],top[2]) for top in Top]
end

#Ensemble based on configuration
function ensemble_cfft(knc,k::Int64=7;testratio=0.4,distance=:hamming_distance)::Vector{Tuple{Tuple{Any,Net},Float64,String}}
    (cl,n),opv,desc=knc[1]
    data=Vector{Vector{String}}(undef, length(knc))
    for i in 1:length(knc)
        kc,opv,desc=knc[i]
        v=split(desc,"/")[1:6]
        data[i]=v
    end
    ind=[i for (i,x) in enumerate(data)]
    partitions,centers,index=[0 for x in ind],[1],KnnResult(k)
    while length(centers)<k && length(centers)<=length(ind)
        oid,dist=maxmin(data,centers,ind,index,distance,partitions)
        push!(centers,oid)
    end
    knc[centers]
end

#New features based ensemble

function ensemble_pfft(knc,k::Int64=7;trainratio=0.7,distance=:hamming_distance)::Vector{Tuple{Tuple{Any,Net},Float64,String}}
    (cl,N),opv,desc=knc[1]
    n=length(N.data)
    tn=Int(trunc(n*trainratio));
    data=Vector{Vector{Float64}}(undef, length(knc))
    perm=Random.randperm(n)
    ti,vi=perm[1:tn],perm[tn+1:n]
    for i in 1:length(knc)
        (cl,N),opv,desc=knc[i]
        xv=gen_features(N.data[vi],N)
        xt=gen_features(N.data[ti],N)
        y=N.labels[ti]
        data[i]=vcat(predict_test(xt,y,xv,desc,cl)...)
    end
    ind=[i for (i,x) in enumerate(data)]
    partitions,centers,index=[0 for x in ind],[1],KnnResult(k)
    while length(centers)<k && length(centers)<=length(ind)
        oid,dist=maxmin(data,centers,ind,index,distance,partitions)
        push!(centers,oid)
    end
    knc[centers]
end

function predict(knc,X;ensemble_k=1)
    y_t=Vector{Int}(undef, 0)
    #@show length(knc)
    for i in 1:ensemble_k
        #kc,opv,desc=knc[i].cl,knc[i].opval, knc[i].ckey
        #cl,N=kc
        cl,N,opv,desc=knc[i].cl,knc[i].net,knc[i].opval, knc[i].ckey
        xv=gen_features(X,N)
        #if contains(desc,"KNN")
        #    y_i=[predict_one(cl,x)[1] for x in xv]
        #else
        #y_i=cl[:predict](xv)
        y_i=cl.predict(xv)
        #end
        y_t = length(y_t)>0 ? hcat(y_t,y_i) : hcat(y_i)
    end
    if length(y_t[:,1])>1
        y_pred=Vector{Int}(undef, length(X))
        for i in 1:length(y_t[:,1])
            y_r=y_t[i,:]
            y_pred[i]=last(sort([(count(x->x==k,y_r),k) for k in unique(y_r)]))[2]
        end
        y_pred
    else
        y_t[:,1]
    end
end

function predict_proba(knc,X):Vector{Vector{Float64}}
    kc,opv,desc=knc
    cl,N=kc
    xv=gen_features(X,N)
    if contains(desc,"KNN")
        y_pred=[predict_one_proba(cl,x) for x in xv]
    elseif  contains(desc,"SVC")
        y_tmp=cl[:decision_function](xv)
        y_pred=[[0,1/(1+exp(-yp))] for yp in y_tmp]
    else
        #yp=cl[:predict_proba](xv)
        yp=cl.predict_proba(xv)
        y_pred=[yp[i,:] for i in 1:length(yp[:,1])]
    end
    y_pred
end

function predict_two(knc,X;ensemble_k=5)::Vector{Int64}
    labels,y_pred,Xt,Xv=[],[],[],[]
    for i in 1:ensemble_k
        kc,opv,desc=knc[i]
        cl,N=kc
        labels=N.labels
        xv=predict_proba(knc[i],X)
        xt=predict_proba(knc[i],N.data)
        #@show xv
        Xv=length(Xv) == 0 ? xv : [vcat(a,b) for (a,b) in zip(Xv,xv)]
        Xt=length(Xt) == 0 ? xt : [vcat(a,b) for (a,b) in zip(Xt,xt)]
    end
    cl,(opval,desc)=transductive_pred(Xt,labels)
    if contains(desc,"KNN")
        y_pred=[predict_one(cl,x)[1] for x in Xv]
    else
        #y_pred=cl[:predict](Xv)
        y_pred=cl.predict(Xv)
    end
    y_pred
end

function predict_proba_ensemble(knc,X;ensemble_k=5)::Vector{Float64}
    labels,y_pred,Xt=[],[],[]
    for i in 1:ensemble_k
        kc,opv,desc=knc[i]
        cl,N=kc
        labels=N.labels
        xt=predict([knc[i]],X)
        Xt=length(Xt) == 0 ? xt : hcat(Xt,xt)
    end
    #@show length(Xt[1])
    vcat(sum(Xt,2)/ensemble_k...)
end


text:
module operators_dense

export DenseOperator, full, projector, dm

import Base: ==, +, -, *, /
import ..operators

using Base.LinAlg, Base.Cartesian
using ..bases, ..states, ..operators

"""
    DenseOperator(b1[, b2, data])

Dense array implementation of Operator.

The matrix consisting of complex floats is stored in the `data` field.
"""
mutable struct DenseOperator <: Operator
    basis_l::Basis
    basis_r::Basis
    data::Matrix{Complex128}
    DenseOperator(b1::Basis, b2::Basis, data) = length(b1) == size(data, 1) && length(b2) == size(data, 2) ? new(b1, b2, data) : throw(DimensionMismatch())
end

DenseOperator(b::Basis, data) = DenseOperator(b, b, data)
DenseOperator(b1::Basis, b2::Basis) = DenseOperator(b1, b2, zeros(Complex128, length(b1), length(b2)))
DenseOperator(b::Basis) = DenseOperator(b, b)
DenseOperator(op::Operator) = full(op)

Base.copy(x::DenseOperator) = DenseOperator(x.basis_l, x.basis_r, copy(x.data))

"""
    full(op::Operator)

Convert an arbitrary Operator into a [`DenseOperator`](@ref).
"""
Base.full(x::Operator) = throw(ArgumentError("Conversion from $(typeof(x)) to a DenseOperator not implemented."))
Base.full(x::DenseOperator) = copy(x)

==(x::DenseOperator, y::DenseOperator) = (x.basis_l == y.basis_l) && (x.basis_r == y.basis_r) && (x.data == y.data)

# Arithmetic operations
+(a::DenseOperator, b::DenseOperator) = (check_samebases(a,b); DenseOperator(a.basis_l, a.basis_r, a.data+b.data))

-(a::DenseOperator) = DenseOperator(a.basis_l, a.basis_r, -a.data)
-(a::DenseOperator, b::DenseOperator) = (check_samebases(a,b); DenseOperator(a.basis_l, a.basis_r, a.data-b.data))

*(a::DenseOperator, b::Ket) = (check_multiplicable(a, b); Ket(a.basis_l, a.data*b.data))
*(a::Bra, b::DenseOperator) = (check_multiplicable(a, b); Bra(b.basis_r, b.data.'*a.data))
*(a::DenseOperator, b::DenseOperator) = (check_multiplicable(a, b); DenseOperator(a.basis_l, b.basis_r, a.data*b.data))
*(a::DenseOperator, b::Number) = DenseOperator(a.basis_l, a.basis_r, complex(b)*a.data)
*(a::Number, b::DenseOperator) = DenseOperator(b.basis_l, b.basis_r, complex(a)*b.data)
function *(op1::Operator, op2::DenseOperator)
    check_multiplicable(op1, op2)
    result = DenseOperator(op1.basis_l, op2.basis_r)
    operators.gemm!(Complex(1.), op1, op2, Complex(0.), result)
    return result
end
function *(op1::DenseOperator, op2::Operator)
    check_multiplicable(op1, op2)
    result = DenseOperator(op1.basis_l, op2.basis_r)
    operators.gemm!(Complex(1.), op1, op2, Complex(0.), result)
    return result
end
function *(op::Operator, psi::Ket)
    check_multiplicable(op, psi)
    result = Ket(op.basis_l)
    operators.gemv!(Complex(1.), op, psi, Complex(0.), result)
    return result
end
function *(psi::Bra, op::Operator)
    check_multiplicable(psi, op)
    result = Bra(op.basis_r)
    operators.gemv!(Complex(1.), psi, op, Complex(0.), result)
    return result
end

/(a::DenseOperator, b::Number) = DenseOperator(a.basis_l, a.basis_r, a.data/complex(b))

operators.dagger(x::DenseOperator) = DenseOperator(x.basis_r, x.basis_l, x.data')

operators.ishermitian(A::DenseOperator) = ishermitian(A.data)

operators.tensor(a::DenseOperator, b::DenseOperator) = DenseOperator(tensor(a.basis_l, b.basis_l), tensor(a.basis_r, b.basis_r), kron(b.data, a.data))
"""
    tensor(x::Ket, y::Bra)

Outer product ``|x⟩⟨y|`` of the given states.
"""
operators.tensor(a::Ket, b::Bra) = DenseOperator(a.basis, b.basis, reshape(kron(b.data, a.data), prod(a.basis.shape), prod(b.basis.shape)))

operators.trace(op::DenseOperator) = (check_samebases(op); trace(op.data))

function operators.ptrace(a::DenseOperator, indices::Vector{Int})
    operators.check_ptrace_arguments(a, indices)
    rank = length(a.basis_l.shape)
    result = _ptrace(Val{rank}, a.data, a.basis_l.shape, a.basis_r.shape, indices)
    return DenseOperator(ptrace(a.basis_l, indices), ptrace(a.basis_r, indices), result)
end

function operators.ptrace(psi::Ket, indices::Vector{Int})
    operators.check_ptrace_arguments(psi, indices)
    b = basis(psi)
    b_ = ptrace(b, indices)
    rank = length(b.shape)
    result = _ptrace_ket(Val{rank}, psi.data, b.shape, indices)
    return DenseOperator(b_, b_, result)
end
function operators.ptrace(psi::Bra, indices::Vector{Int})
    operators.check_ptrace_arguments(psi, indices)
    b = basis(psi)
    b_ = ptrace(b, indices)
    rank = length(b.shape)
    result = _ptrace_bra(Val{rank}, psi.data, b.shape, indices)
    return DenseOperator(b_, b_, result)
end

operators.normalize!(op::DenseOperator) = scale!(op.data, 1./trace(op))

function operators.expect(op::DenseOperator, state::Operator)
    check_samebases(op.basis_r, state.basis_l)
    check_samebases(op.basis_l, state.basis_r)
    result = Complex128(0.)
    @inbounds for i=1:size(op.data, 1), j=1:size(op.data,2)
        result += op.data[i,j]*state.data[j,i]
    end
    result
end

function operators.expm(op::DenseOperator)
    check_samebases(op)
    return DenseOperator(op.basis_l, op.basis_r, expm(op.data))
end

function operators.permutesystems(a::DenseOperator, perm::Vector{Int})
    @assert length(a.basis_l.bases) == length(a.basis_r.bases) == length(perm)
    @assert isperm(perm)
    data = reshape(a.data, [a.basis_l.shape; a.basis_r.shape]...)
    data = permutedims(data, [perm; perm + length(perm)])
    data = reshape(data, length(a.basis_l), length(a.basis_r))
    DenseOperator(permutesystems(a.basis_l, perm), permutesystems(a.basis_r, perm), data)
end

operators.identityoperator(::Type{DenseOperator}, b1::Basis, b2::Basis) = DenseOperator(b1, b2, eye(Complex128, length(b1), length(b2)))

"""
    projector(a::Ket, b::Bra)

Projection operator ``|a⟩⟨b|``.
"""
projector(a::Ket, b::Bra) = tensor(a, b)
"""
    projector(a::Ket)

Projection operator ``|a⟩⟨a|``.
"""
projector(a::Ket) = tensor(a, dagger(a))
"""
    projector(a::Bra)

Projection operator ``|a⟩⟨a|``.
"""
projector(a::Bra) = tensor(dagger(a), a)

"""
    dm(a::StateVector)

Create density matrix ``|a⟩⟨a|``. Same as `projector(a)`.
"""
dm(x::Ket) = tensor(x, dagger(x))
dm(x::Bra) = tensor(dagger(x), x)

# Partial trace implementation for dense operators.
function _strides(shape::Vector{Int})
    N = length(shape)
    S = zeros(Int, N)
    S[1] = 1
    for m=2:N
        S[m] = S[m-1]*shape[m-1]
    end
    return S
end

# Dense operator version
@generated function _ptrace(::Type{Val{RANK}}, a::Matrix{Complex128},
                            shape_l::Vector{Int}, shape_r::Vector{Int},
                            indices::Vector{Int}) where RANK
    return quote
        a_strides_l = _strides(shape_l)
        result_shape_l = copy(shape_l)
        result_shape_l[indices] = 1
        result_strides_l = _strides(result_shape_l)
        a_strides_r = _strides(shape_r)
        result_shape_r = copy(shape_r)
        result_shape_r[indices] = 1
        result_strides_r = _strides(result_shape_r)
        N_result_l = prod(result_shape_l)
        N_result_r = prod(result_shape_r)
        result = zeros(Complex128, N_result_l, N_result_r)
        @nexprs 1 (d->(Jr_{$RANK}=1;Ir_{$RANK}=1))
        @nloops $RANK ir (d->1:shape_r[d]) (d->(Ir_{d-1}=Ir_d; Jr_{d-1}=Jr_d)) (d->(Ir_d+=a_strides_r[d]; if !(d in indices) Jr_d+=result_strides_r[d] end)) begin
            @nexprs 1 (d->(Jl_{$RANK}=1;Il_{$RANK}=1))
            @nloops $RANK il (k->1:shape_l[k]) (k->(Il_{k-1}=Il_k; Jl_{k-1}=Jl_k; if (k in indices && il_k!=ir_k) Il_k+=a_strides_l[k]; continue end)) (k->(Il_k+=a_strides_l[k]; if !(k in indices) Jl_k+=result_strides_l[k] end)) begin
                result[Jl_0, Jr_0] += a[Il_0, Ir_0]
            end
        end
        return result
    end
end

@generated function _ptrace_ket(::Type{Val{RANK}}, a::Vector{Complex128},
                            shape::Vector{Int}, indices::Vector{Int}) where RANK
    return quote
        a_strides = _strides(shape)
        result_shape = copy(shape)
        result_shape[indices] = 1
        result_strides = _strides(result_shape)
        N_result = prod(result_shape)
        result = zeros(Complex128, N_result, N_result)
        @nexprs 1 (d->(Jr_{$RANK}=1;Ir_{$RANK}=1))
        @nloops $RANK ir (d->1:shape[d]) (d->(Ir_{d-1}=Ir_d; Jr_{d-1}=Jr_d)) (d->(Ir_d+=a_strides[d]; if !(d in indices) Jr_d+=result_strides[d] end)) begin
            @nexprs 1 (d->(Jl_{$RANK}=1;Il_{$RANK}=1))
            @nloops $RANK il (k->1:shape[k]) (k->(Il_{k-1}=Il_k; Jl_{k-1}=Jl_k; if (k in indices && il_k!=ir_k) Il_k+=a_strides[k]; continue end)) (k->(Il_k+=a_strides[k]; if !(k in indices) Jl_k+=result_strides[k] end)) begin
                result[Jl_0, Jr_0] += a[Il_0]*conj(a[Ir_0])
            end
        end
        return result
    end
end

@generated function _ptrace_bra(::Type{Val{RANK}}, a::Vector{Complex128},
                            shape::Vector{Int}, indices::Vector{Int}) where RANK
    return quote
        a_strides = _strides(shape)
        result_shape = copy(shape)
        result_shape[indices] = 1
        result_strides = _strides(result_shape)
        N_result = prod(result_shape)
        result = zeros(Complex128, N_result, N_result)
        @nexprs 1 (d->(Jr_{$RANK}=1;Ir_{$RANK}=1))
        @nloops $RANK ir (d->1:shape[d]) (d->(Ir_{d-1}=Ir_d; Jr_{d-1}=Jr_d)) (d->(Ir_d+=a_strides[d]; if !(d in indices) Jr_d+=result_strides[d] end)) begin
            @nexprs 1 (d->(Jl_{$RANK}=1;Il_{$RANK}=1))
            @nloops $RANK il (k->1:shape[k]) (k->(Il_{k-1}=Il_k; Jl_{k-1}=Jl_k; if (k in indices && il_k!=ir_k) Il_k+=a_strides[k]; continue end)) (k->(Il_k+=a_strides[k]; if !(k in indices) Jl_k+=result_strides[k] end)) begin
                result[Jl_0, Jr_0] += conj(a[Il_0])*a[Ir_0]
            end
        end
        return result
    end
end

# Fast in-place multiplication
operators.gemm!(alpha, a::Matrix{Complex128}, b::Matrix{Complex128}, beta, result::Matrix{Complex128}) = BLAS.gemm!('N', 'N', convert(Complex128, alpha), a, b, convert(Complex128, beta), result)
operators.gemv!(alpha, a::Matrix{Complex128}, b::Vector{Complex128}, beta, result::Vector{Complex128}) = BLAS.gemv!('N', convert(Complex128, alpha), a, b, convert(Complex128, beta), result)
operators.gemv!(alpha, a::Vector{Complex128}, b::Matrix{Complex128}, beta, result::Vector{Complex128}) = BLAS.gemv!('T', convert(Complex128, alpha), b, a, convert(Complex128, beta), result)

operators.gemm!(alpha, a::DenseOperator, b::DenseOperator, beta, result::DenseOperator) = operators.gemm!(convert(Complex128, alpha), a.data, b.data, convert(Complex128, beta), result.data)
operators.gemv!(alpha, a::DenseOperator, b::Ket, beta, result::Ket) = operators.gemv!(convert(Complex128, alpha), a.data, b.data, convert(Complex128, beta), result.data)
operators.gemv!(alpha, a::Bra, b::DenseOperator, beta, result::Bra) = operators.gemv!(convert(Complex128, alpha), a.data, b.data, convert(Complex128, beta), result.data)

# Multiplication for Operators in terms of their gemv! implementation
function operators.gemm!(alpha, M::Operator, b::DenseOperator, beta, result::DenseOperator)
    for i=1:size(b.data, 2)
        bket = Ket(b.basis_l, b.data[:,i])
        resultket = Ket(M.basis_l, result.data[:,i])
        operators.gemv!(alpha, M, bket, beta, resultket)
        result.data[:,i] = resultket.data
    end
end

function operators.gemm!(alpha, b::DenseOperator, M::Operator, beta, result::DenseOperator)
    for i=1:size(b.data, 1)
        bbra = Bra(b.basis_r, vec(b.data[i,:]))
        resultbra = Bra(M.basis_r, vec(result.data[i,:]))
        operators.gemv!(alpha, bbra, M, beta, resultbra)
        result.data[i,:] = resultbra.data
    end
end

end # module

ext: jl


text:
struct DirectSolver <: LinearSolver end

scsint_t(::Type{DirectSolver}) = Int

function scs_set_default_settings(::Type{DirectSolver}, data::ScsData{Int})
    return ccall(
        (:scs_set_default_settings, direct),
        Cvoid,
        (Ptr{Cvoid},),
        data,
    )
end

function scs_init(
    ::Type{DirectSolver},
    data::ScsData{Int},
    cone::ScsCone{Int},
    info::ScsInfo{Int},
)
    return ccall(
        (:scs_init, direct),
        Ptr{Cvoid},
        (Ptr{Cvoid}, Ptr{Cvoid}, Ptr{Cvoid}),
        data,
        cone,
        info,
    )
end

function scs_solve(
    ::Type{DirectSolver},
    p_work::Ptr{Cvoid},
    data::ScsData{Int},
    cone::ScsCone{Int},
    solution::ScsSolution,
    info::ScsInfo{Int},
)
    return ccall(
        (:scs_solve, direct),
        Int,
        (Ptr{Cvoid}, Ptr{Cvoid}, Ptr{Cvoid}, Ptr{Cvoid}, Ptr{Cvoid}),
        p_work,
        data,
        cone,
        solution,
        info,
    )
end

function scs_finish(::Type{DirectSolver}, p_work::Ptr{Cvoid})
    return ccall((:scs_finish, direct), Cvoid, (Ptr{Cvoid},), p_work)
end

function scs_version()
    return unsafe_string(ccall((:scs_version, direct), Cstring, ()))
end


text:
module GRUtils

import GR
using LinearAlgebra

export Figure, gcf, subplot, currentplot, draw, savefig, video, videofile,
    plot, plot3, polar, quiver, quiver3, scatter, scatter3, stair, stem, oplot,
    errorbar, barplot, histogram, polarhistogram,
    contour, contourf, shade, surface, tricont, trisurf, volume, wireframe,
    heatmap, polarheatmap, hexbin, imshow, isosurface, annotations,
    aspectratio, colorbar, geometrykinds, grid, hold, legend, radians,
    title, panzoom, zoom, viewpoint, rotate, tilt, movefocus, turncamera,
    xflip, xlabel, xlim, xlog, xticks, xticklabels,
    yflip, ylabel, ylim, ylog, yticks, yticklabels,
    zflip, zlabel, zlim, zlog, zticks,
    colormap, colorscheme,

    subplot!, plot!, plot3!, polar!, quiver!, quiver3!, scatter!, scatter3!, stair!, stem!, oplot!,
    errorbar!, barplot!, histogram!, polarhistogram!,
    contour!, contourf!, shade!, surface!, tricont!, trisurf!, volume!, wireframe!,
    heatmap!, polarheatmap!, hexbin!, imshow!, isosurface!, annotations!,
    aspectratio!, colorbar!, grid!, hold!, legend!, title!,
    radians!, panzoom!, zoom!, viewpoint!, rotate!, tilt!, movefocus!, turncamera!,
    xflip!, xlabel!, xlim!, xlog!, xticks!, xticklabels!,
    yflip!, ylabel!, ylim!, ylog!, yticks!, yticklabels!,
    zflip!, zlabel!, zlim!, zlog!, zticks!,
    colormap!, colorscheme!

include("general.jl")
include("colors.jl")
include("geometries.jl")
include("axes.jl")
include("legends.jl")
include("colorbars.jl")
include("plotobjects.jl")
include("text.jl")
include("figures.jl")
include("frontend.jl")
include("attributes.jl")
include("animations.jl")

end # module


text:
# Clusters
# ========

# Clustering.jl : ClusteringResult

immutable NoClustering <: ClusteringResult end

"""
Data structure to represent sequence clusters.
The sequence data itself is not included.
"""
immutable SequenceClusters <: ClusteringResult
    clustersize::Vector{Int}
    sequencecluster::Vector{Int}
    sequenceweight::Vector{Float64}
end

nsequences(cl::SequenceClusters) = length(cl.sequencecluster)

# Clustering.jl : nclusters, counts, assignments

"Get the number of clusters in a `SequenceClusters` object."
nclusters(cl::SequenceClusters) = length(cl.clustersize)

"Get sample counts of clusters as a `Vector`. Each `k` value is the number of samples assigned to the k-th cluster."
counts(cl::SequenceClusters) = cl.clustersize

"Get a vector of assignments, where the `i` value is the index/number of the cluster to which the i-th sequence is assigned."
assignments(cl::SequenceClusters) = cl.sequencecluster

#  convert from ClusteringResult of Clustering.jl to SequenceClusters
# -------------------------------------------------------------------

# needs tests
function convert(::Type{SequenceClusters}, cl::ClusteringResult)
    clustersize = counts(cl)
    sequencecluster = assignments(cl)
    sequenceweight = Float64[ 1.0/clustersize[k] for k in sequencecluster ]
    SequenceClusters(clustersize, sequencecluster, sequenceweight)
end

convert(::Type{SequenceClusters}, cl::SequenceClusters) = cl # no-op

# weights
# -------

"""```getweight(c[, i::Int])```

This function returns the weight of the sequence number `i`.
getweight should be defined for any type used for `count!`/`count` in order to use his weigths.
If `i` isn't used, this function returns a vector with the weight of each sequence.
"""
getweight(c::SequenceClusters, seq::Int) = c.sequenceweight[seq]

@inline getweight(weight::NoClustering, i::Int) = 1

getweight(weights::AbstractVector, i::Int) = weights[i]

getweight(cl::SequenceClusters) = cl.sequenceweight

getweight(cl::ClusteringResult) = getweight(convert(SequenceClusters, cl))


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

using Conda, PyCall

module sail_route

include("route/domain.jl")
include("weather/load_weather.jl")
include("scenarios/comparison.jl")
include("performance/aerodynamics.jl")
include("performance/polar.jl")

end


text:
include( "lossFunctions.jl" )

using Distributions
using LinearAlgebra
using ProgressMeter
using Match

  """
      tobin( num::Int64 )

  """
  function tobin( num::Int64 )
    @match num begin
      0 => "0"
      1 => "1"
      _ => string( tobin( div( num, 2 ) ), mod( num, 2 ) )
    end
  end

  """
      GMatrix( d::Int64 )

      compute transformation matrix G.
      #Arguments
      - `d::Int64`:        number of hypothesis used for prediction.
      #Return
      - `Matrix{Float64}`: transformation matrix G.
  """
  function GMatrix( d::Int64 )
    N   = 2^d;
    mat = Matrix{Float64}( undef, d, N );
    for row in 1:1:N
      mat[:,row] = parse.( Float64, split( lpad( tobin( row - 1 ), d, "0" ), "" ) );
    end
    return mat;
  end

  """
      dirichletPosteriorEstimation( errMat::Matrix{Float64}, G::Matrix{Float64}, nrRuns::Int64, α_::Float64 )

      compute posterior p( h* = h | S ).
      # Arguments
      - `errMat::Matrix{Float64}`: each column is the prediction error of one hypothesis.
      - `G::Matrix{Float64}`:      transformation matrix G.
      - `nrRuns::Int64`:           number of sampling runs.
      - `α_::Float64`:             scalar prior parameter.
      - `sampleSize::Int64`:       number of samples per run.
      # Return
      - `Vector{Float64}`:         posterior distribution
  """
  function dirichletPosteriorEstimation( errMat::Matrix{Float64}, G::Matrix{Float64}, nrRuns::Int64, α_::Float64 )
    ## number of prediction models to combine
    m = size( errMat )[1];
    d = size( errMat )[2];
    ## number outcomes
    N = 2^d;
    ## counts outcomes
    K   = zeros( Float64, N );
    ## init resulting posterior distribution
    res = zeros( Float64, d );
    ## multivariate prior parameter
    α = α_ * ones( Float64, N );

    ## compute count OF OCCURENCES K
    @showprogress 1 "Computing..." for i in 1:1:m
      col     = parse( Int64, join( string.( convert( Array{Int64,1}, errMat[i,:] ) ) ), base=2 ) + 1;
      K[col] += 1;
    end
    K ./= sum( K );

    ## ML- Distribution
    βDist    = Beta( α_ * N, m );
    ## prior Distribution
    αDirDist = Dirichlet( α );
    ## ML- Distribution
    KDirDist = Dirichlet( K .+ 1e-9 );

    @showprogress 1 "Computing..." for i in 1:1:nrRuns
      θSample     = rand( βDist );
      αSample     = rand( αDirDist );
      KSample     = rand( KDirDist );
      q           = ( θSample .* αSample ) .+ ( ( 1 - θSample ) * KSample );
      r           = G * q;
      label       = argminUniProb.argminProb( r );
      res[label] += 1.0;
    end
    return  res ./ nrRuns;
  end

  """
      dirichletPosteriorEstimationV2( errMat::Matrix{Float64}, G::Matrix{Float64}, nrRuns::Int64, α_::Float64, sampleSize::Int64 )

      compute posterior p( h* = h | S ), alternative version for improved performance.
      # Arguments
      - `errMat::Matrix{Float64}`: each column is the prediction error of one hypothesis.
      - `G::Matrix{Float64}`:      transformation matrix G.
      - `nrRuns::Int64`:           number of sampling runs.
      - `α_::Float64`:             scalar prior parameter.
      - `sampleSize::Int64`:       number of samples per run.
      # Return
      - `Vector{Float64}`:         posterior distribution posterior p( h* = h | S ).
  """
  function dirichletPosteriorEstimationV2( errMat::Matrix{Float64}, G::Matrix{Float64}, nrRuns::Int64, α_::Float64, sampleSize::Int64 )
    ## number of prediction models to combine
    m = size( errMat, 1 );
    d = size( errMat, 2 );
    ## number outcomes
    N = 2^d;
    ## counts outcomes
    K   = zeros( Float64, N );
    ## init resulting posterior distribution
    res = zeros( Float64, d );
    ## multivariate prior parameter
    α = α_ * ones( Float64, N );

    ## compute count OF OCCURENCES K
    for i in 1:1:m
      col     = parse( Int64, join( string.( convert( Array{Int64,1}, errMat[i,:] ) ) ), base=2 ) + 1;
      K[col] += 1;
    end
    K ./= sum( K );

    ## ML- Distribution
    βDist    = Beta( α_ * N, m );
    ## prior Distribution
    αDirDist = Dirichlet( α );
    ## ML- Distribution
    KDirDist = Dirichlet( K .+ 1e-9 );

    labels = zeros( Int64, sampleSize );
    for i in 1:1:nrRuns
      θSample     = rand( βDist,    sampleSize );
      αSample     = rand( αDirDist, sampleSize );
      KSample     = rand( KDirDist, sampleSize );
      for i in 1:1:sampleSize
        for j in 1:1:N
          αSample[j,i] *= θSample[i];
          KSample[j,i] *= ( 1.0 - θSample[i] );
        end
      end
      q           = αSample .+ KSample;
      r           = G * q;
      argminUniProb.argminProb!( r, labels );
      for pos in labels
        res[pos] += 1.0;
      end
    end
    return  res ./ sum( res );
  end

  """
      dirichletPosteriorEstimation( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64 )

      compute posterior p( h* = h | S ).
      #Arguments
      - `errMat::Matrix{Float64}`: each column is the prediction error of one hypothesis.
      - `nrRuns::Int64`:           number of main  iterations.
      - `α_::Float64`:             scalar prior parameter.
      #Return
      - `Vector{Float64}`:         posterior p( h* = h | S ).
  """
  function dirichletPosteriorEstimation( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64 )
    d = size( errMat )[2];
    G = GMatrix( d );
    return dirichletPosteriorEstimation( errMat, G, nrRuns, α_ );
  end

  """
      dirichletPosteriorEstimationV2( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64, sampleSize::Int64 )

      compute posterior p( h* = h | S ), alternative version for improved performance.
      # Arguments
      - `errMat::Matrix{Float64}`: each column is the prediction of one hypothesis.
      - `nrRuns::Int64`:           number of sampling runs.
      - `α_::Float64`:             scalar prior parameter.
      - `sampleSize::Int64`:       number of samples per run.
      # Return
      - `Vector{Float64}`:         posterior distribution p( h* = h | S ).
  """
  function dirichletPosteriorEstimationV2( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64, sampleSize::Int64 )
    d = size( errMat )[2];
    G = GMatrix( d );
    return dirichletPosteriorEstimationV2( errMat, G, nrRuns, α_, sampleSize );
  end

  """
      dirichletPosteriorEstimation!( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64, p::Vector{Float64} )

      compute posterior p( h* = h | S ).
      #Arguments
      - `errMat::Matrix{Float64}`: each column is the prediction error of one hypothesis.
      - `nrRuns::Int64`:           number of passes over predictions.
      - `α_::Float64`:             meta parameter value.
      - `p::Vector{Float64}`:      return value posterior p( h* = h | S ).
      #Return
      - `nothing`:                 nothing.
  """
  function dirichletPosteriorEstimation!( errMat::Matrix{Float64}, nrRuns::Int64, α_::Float64, p::Vector{Float64} )
    pRes = dirichletPosteriorEstimation( errMat, nrRuns, α_ );
    for (i,val) in enumerate( pRes )
      p[i] = val;
    end
  end

  """
      metaParamSearchValidationDirichlet( Y::Matrix{Float64}, t::Vector{Float64}, nrRuns::Int64, minVal::Float64, maxVal::Float64, nSteps::Int64, holdout::Float64, lossFunc )

      compute best α parameter regarding predictive performance.
      #Arguments
      - `Y::Matrix{Float64}`: each column is the prediction error of one hypothesis.
      - `t::Vector{Float64}`: label vector.
      - `nrRuns::Int64`:      number of passes over predictions.
      - `minVal::Float64`:    minimum value of α.
      - `maxVal::Float64`:    maximum value of α.
      - `nSteps::Int64`:      number of steps between min and max val.
      - `holdout::Float64`:   percentage used in holdout.
      - `lossFunc`:           error function handle.
      #Return
      - `Vector{Float64} x2`: α_sequence, performance_sequence.
  """
  function metaParamSearchValidationDirichlet( Y::Matrix{Float64}, t::Vector{Float64}, nrRuns::Int64, minVal::Float64, maxVal::Float64, nSteps::Int64, holdout::Float64, lossFunc )
    @assert nrRuns > 0
    @assert maxVal > minVal
    @assert minVal >= 0.0 && maxVal > 0.0
    @assert nSteps > 1
    @assert size( Y, 1 ) == size( t, 1 )
    @assert holdout * size( Y, 1 ) > 1.0

    limit      = Int64( holdout * size( Y, 1 ) );
    YTrain     = Y[limit+1:end,:];
    tTrain     = t[limit+1:end];
    YEval      = Y[1:limit,:];
    tEval      = t[1:limit];
    errMat     = ( repeat(  Float64.( tTrain ), outer = [ 1, size( YTrain, 2 ) ] ) .- YTrain ).^2;
    αSequence  = collect( minVal:(maxVal-minVal)/nSteps:maxVal );
    performace = Float64[]
    for α in αSequence
      posterior   = dirichletPosteriorEstimation( errMat, nrRuns, α );
      yPrediction = YEval * posterior;
      push!( performace, mean( lossFunc( yPrediction, tEval ) ) );
    end
    return αSequence, performace
  end


text:
"""
Utility functions for Tensor Graphical Models

Author: Wayne Wang
Last modified: 07/14/2021
"""

function kroneckersum_alt(A::AbstractMatrix, B::AbstractMatrix)
    """
    Alternate Kronecker sum defined as I ⊗ A + B ⊗ I
    """
    return kroneckersum(B,A)
end

function kroneckersum_list(mat_list::AbstractVector{<:Any})
    """
    Kronecker sum for a list of matrices using package Kronecker.jl
    In
        - mat_list::AbstractVector{<:Any}: a list of matrices
    Out
        - mat::KroneckerSum: mat_list[1] ⊕ ... ⊕ mat_list[end]
    """
    K = length(mat_list)
    mat = copy(mat_list[1])
    if K == 1
        return mat
    else
        for j=2:K
            mat = kroneckersum_alt(mat, mat_list[j])
        end
    end
    return mat
end

function mul_alt!(C::StridedMatrix, X::StridedMatrix, A::SparseMatrixCSC)
    mX, nX = size(X)
    nX == A.m || throw(DimensionMismatch())
    fill!(C, zero(eltype(C)))
    rowval = A.rowval
    nzval = A.nzval
    @inbounds for  col = 1:A.n, k=A.colptr[col]:(A.colptr[col+1]-1)
        ki=rowval[k]
        kv=nzval[k]
        for multivec_row=1:mX
            C[multivec_row, col] += X[multivec_row, ki] * kv
        end
    end
    C
end

import Base.*
function *(B::StridedMatrix, A::SparseMatrixCSC)
    mB, nB = size(B)
    nB == A.m || throw(DimensionMismatch())
    C = zeros(mB,A.n)
    rowval = A.rowval
    nzval = A.nzval
    @inbounds for  col = 1:A.n, k=A.colptr[col]:(A.colptr[col+1]-1)
        ki=rowval[k]
        kv=nzval[k]
        for multivec_row=1:mB
            C[multivec_row, col] += B[multivec_row, ki] * kv
        end
    end
    C
end

function offdiag(X::AbstractMatrix)
    """
    Extract the off-diagonal matrix of X
    """
    return X - Diagonal(X)
end

function offdiag!(X::AbstractMatrix)
    """
    In placce version of offdiag
    """
    X .= X .- Diagonal(X)
    return X
end


text:
using Test
include("../src/WJFKNN.jl")
#import WJFKNN:WjfKNN, knnSearch
using WJFParallelTask

begin
data = Vector{Vector{Float64}}()
numClusters = 20
numDims = 10
k = UInt32(10)
clusterCenters = Vector{Vector{Float64}}()
clusterSizes = Vector{Vector{Float64}}()
for i = 1:numClusters
    push!(clusterCenters, rand(Float64, numDims))
    push!(clusterSizes, rand(Float64, numDims))
end

numData = 100000
for i = 1:numData
    clusterID = rand(1:numClusters)
    position = clusterCenters[clusterID] .+
        rand(Float64, numDims) .* clusterSizes[clusterID]
    push!(data, position)
end

normalizedData = [v./sqrt(sum(v.*v)) for v in data]
function euclidDist(u::Vector{Float64}, v::Vector{Float64})
    return sqrt(sum((u .- v).^2))
end

function cosDist(u::Vector{Float64}, v::Vector{Float64})
    return 0.5 - 0.5 * sum(u.*v)
end

function baselineKNN(distFun::Function)::Vector{Vector{Tuple{Float64,UInt32}}}
    result = Vector{Vector{Tuple{Float64,UInt32}}}(undef, numData)
    myData = if distFun == cosDist
        normalizedData
    else
        data
    end
    function mapFun(u1::UInt32, u2::UInt32)
        dists = Vector{Tuple{Float64,UInt32}}(undef, numData)
        for i::UInt32 = u1:u2
            for j::UInt32 = 1:numData
                dists[j] = (distFun(myData[i], myData[j]), j)
            end
            dists[i] = (Inf64, i)
            sort!(dists)
            result[i] = dists[1:k]
        end # i
    end
    mapOnly(UInt32(1),UInt32(numData),UInt32(100),mapFun)
    return result
end

function wjfKNNEuclid()::Vector{Vector{Tuple{Float64,UInt32}}}
    wjfKnn = WJFKNN.WjfKNN(data, euclidDist)
    return WJFKNN.knnSearch(wjfKnn, k, 0.5)
end

function wjfKNNCos()::Vector{Vector{Tuple{Float64,UInt32}}}
    wjfKnn = WJFKNN.WjfKNN(normalizedData, cosDist)
    return WJFKNN.knnSearch(wjfKnn, k, 0.5)
end

function computeAccuracy(a::Vector{Vector{Tuple{Float64,UInt32}}}, b::Vector{Vector{Tuple{Float64,UInt32}}})
    accuracy = 0.0
    for i = 1:numData
        sa = Set([x[2] for x in a[i]])
        sb = Set([x[2] for x in b[i]])
        accuracy += length(intersect(sa, sb))
    end
    accuracy /= (numData * k)
    return accuracy
end

@testset "cos distance knn" begin
    @time a = baselineKNN(cosDist)
    for round = 1:10
        println("round = $round")
        @time b = wjfKNNCos()
        @test computeAccuracy(a, b) > 0.85
    end
end

@testset "euclid distance knn" begin
    @time a = baselineKNN(euclidDist)
    for round = 1:10
        println("round = $round")
        @time b = wjfKNNEuclid()
        @test computeAccuracy(a, b) > 0.9
    end
end

end


text:
using NullableArrays
using PyCall
@pyimport numpy.ma as ma
PyObject(a::NullableArray) =  pycall(ma.array, Any, a.values, mask=a.isnull)

function interior_pcolor(xx, yy, ff, interior_mask; vmin=nothing, vmax=nothing, cmap=nothing)
    P = NullableArray(ff, .!reshape(interior_mask, size(ff)))
    pcolormesh(xx, yy, PyObject(P), vmin=vmin, vmax=vmax, cmap=cmap)
end

function plot_boundary(dcurve, style="-k")
    Ncurves = dcurve.curvenum[end]
    for n=1:Ncurves
        idx = findin(dcurve.curvenum, n)
        plot(dcurve.points[1,idx], dcurve.points[2,idx], style)
    end
    axis("equal")
end

function zoom_to_boundary(dcurve)
    xmin, xmax = extrema(dcurve.points[1,:]) .+ (-0.01, 0.01)
    ymin, ymax = extrema(dcurve.points[2,:]) .+ (-0.01, 0.01)
    axis((xmin, xmax, ymin, ymax))
end


text:
retrieve(c::Client, ::Type{GuildEmbed}, g::AbstractGuild) = get_guild_embed(c, g.id)

update(c::Client, ::GuildEmbed, g::AbstractGuild; kwargs...) = modify_guild_embed(c, g.id; kwargs...)


text:
function main(r::Robot)
    for side in (Nord, West, Sud, Ost)
        num_steps = put_marker_in_direction_and_get_num_steps(r,side)
        try_move(r,inverse(side), num_steps)
    end
    putmarker!(r)
end

function put_marker_in_direction_and_get_num_steps(r::Robot,side::HorizonSide)
    num_steps=0
    while try_move(r, side)
        putmarker!(r)
        num_steps += 1
    end
    return num_steps
end

try_move(r::Robot, side::HorizonSide, num_steps::Int) =
for _ in 1:num_steps
    try_move(r,side)
end

function try_move(r::Robot, direct_side::HorizonSide)::Bool
    orthogonal_side = next_clockwise(direct_side)
    reverse_side = inverse(orthogonal_side)
    num_steps=0
    if !isborder(r,direct_side)
        move!(r,direct_side)
        result=true
    else
        while isborder(r,direct_side)
            if !isborder(r, orthogonal_side)
                move!(r, orthogonal_side)
                num_steps += 1
            else
                break
            end
        end
        if !isborder(r,direct_side)
            move!(r,direct_side)
            while isborder(r,reverse_side)
                move!(r,direct_side)
            end
            result = true
        else
            result = false
        end
        while num_steps>0
            num_steps=num_steps-1
            move!(r,reverse_side)
        end
    end
    return result
end

next_clockwise(side::HorizonSide) = HorizonSide(mod(Int(side)-1,4))
inverse(side::HorizonSide) = HorizonSide(mod(Int(side)+2, 4))

max_stars_repo_name: VladislavBukin/mirea-progs

max_issues_repo_name: VladislavBukin/mirea-progs

max_forks_repo_name: VladislavBukin/mirea-progs


text:
# __precompile__()

"""
Multi-Dimensional Bisection Method (MDBM) is an efficient and robust root-finding algorithm,
which can be used to determine whole high-dimensional sub-manifolds (points, curves, surfaces…)
of the roots of implicit non-linear equation systems, even in cases, where the number of unknowns
surpasses the number of equations.

# Examples
```julia
include("MDBM.jl")
using Reexport
@reexport using .MDBM

using PyPlot;
pygui(true);

function foo(x,y)
    x^2.0+y^2.0-2.0^2.0
end
function c(x,y) #only the c>0 domain is analysed
    x-y
end

ax1=Axis([-5,-2.5,0,2.5,5],"x") # initial grid in x direction
ax2=Axis(-5:2:5.0,"y") # initial grid in y direction

mymdbm=MDBM_Problem(foo,[ax1,ax2],constraint=c)
iteration=5 #number of refinements (resolution doubling)
solve!(mymdbm,iteration)

#points where the function foo was evaluated
x_eval,y_eval=getevaluatedpoints(mymdbm)

#interpolated points of the solution (approximately where foo(x,y) == 0 and c(x,y)>0)
x_sol,y_sol=getinterpolatedsolution(mymdbm)

fig = figure(1);clf()
scatter(x_eval,y_eval,s=5)
scatter(x_sol,y_sol,s=5)
```
"""
module MDBM
using StaticArrays
using LinearAlgebra

export MDBM_Problem, Axis,
 solve!, interpolate!, refine!, checkneighbour!,
  axesextend!, getinterpolatedsolution, getevaluatedpoints, getevaluatedfunctionvalues, getevaluatedconstraintvalues,
 connect, triangulation

include("MDBM_types.jl")
include("MDBM_functions.jl")

end


text:
macro ode_def(name,ex,params...)
    opts = Dict{Symbol,Bool}(
    :build_tgrad => true,
    :build_jac => true,
    :build_expjac => false,
    :build_invjac => false,
    :build_invW => false,
    :build_hes => false,
    :build_invhes => false,
    :build_dpfuncs => true)
    name isa Expr ? ode_def_opts(gensym(),opts,__module__,name,ex,params...) :
        ode_def_opts(name,opts,__module__,ex,params...)
end

macro ode_def_bare(name,ex,params...)
    opts = Dict{Symbol,Bool}(
    :build_tgrad => false,
    :build_jac => false,
    :build_expjac => false,
    :build_invjac => false,
    :build_invW => false,
    :build_hes => false,
    :build_invhes => false,
    :build_dpfuncs => false)
    name isa Expr ? ode_def_opts(gensym(),opts,__module__,name,ex,params...) :
        ode_def_opts(name,opts,__module__,ex,params...)
end

macro ode_def_all(name,ex,params...)
    opts = Dict{Symbol,Bool}(
    :build_tgrad => true,
    :build_jac => true,
    :build_expjac => false,
    :build_invjac => false,
    :build_invW => true,
    :build_hes => false,
    :build_invhes => false,
    :build_dpfuncs => true)
    name isa Expr ? ode_def_opts(gensym(),opts,__module__,name,ex,params...) :
        ode_def_opts(name,opts,__module__,ex,params...)
end


text:
module ActuatorArrayModule

export ActuatorArray, actuatorPosition, closestActuator, genActList,
    actuatorsInCommon

struct ActuatorArray{T<:Real, U<:Unsigned}
    nx::U # Number of actuators along x-axis
    ny::U # Number of actuators along y-axis
    dx::T # Spacing of the actuators (it is assumed that the spacing is the same along both directions)
    width::T # Width of one actuator
    shape::Symbol # shape of the actuator (either :cicrcle, :square or :coil)
    ignore_list::Array{Tuple{U,U}, 1} # list of ignored actuators

    function ActuatorArray(nx::U, ny::U, dx::T, width::T=dx, shape::Symbol=:circle, ignore_list::Array{Tuple{U,U}, 1} = Array{Tuple{U,U}, 1}())  where {T<:Real, U<:Unsigned}
        new{T, U}(nx, ny, dx, width, shape, ignore_list)
    end

    function ActuatorArray(nx::I, ny::I, dx::T, width::T=dx, shape::Symbol=:circle, ignore_list::Array{Tuple{I,I}, 1} = Array{Tuple{I, I}, 1}())  where {T<:Real, I<:Int}
        @assert (nx > 1 && ny > 1) "The number of actuators along x and y axis must be positive."
        # convert the ignore list from signed int to unsigned ints
        ignore_list_U = [(unsigned(a[1]), unsigned(a[2])) for a in ignore_list];
        new{T, unsigned(I)}(unsigned(nx), unsigned(ny), dx, width, shape, ignore_list_U)
    end
end

function Base.iterate(aa::ActuatorArray{T, U}, state=1) where {T<:Real, U<:Unsigned}
    if state <= aa.nx*aa.ny
        return ( (U((state-1)%aa.nx + 1), U(div(state-1, aa.nx)+1)), state+1);
    else
        return nothing;
    end
end

function actuatorPosition(aa::ActuatorArray{T,U}, indexes::Tuple{U, U}) where {T<:Real, U<:Unsigned}
    # returns a position of the (i,j)-th coil
    x = (indexes[1]-oneunit(aa.dx))*aa.dx;
    y = (indexes[2]-oneunit(aa.dx))*aa.dx;
    return x, y
end

function closestActuator(aa::ActuatorArray{T,U}, pos::Tuple{T, T}) where {T<:Real, U<:Unsigned}
    i = round(U, pos[1]/aa.dx + oneunit(aa.dx));
    j = round(U, pos[2]/aa.dx + oneunit(aa.dx));
    return i, j
end

function genActList(aa::ActuatorArray{T,U}, objPos::Tuple{T,T,T}, maxDist_used, maxDist_considered) where {T<:Real, U<:Unsigned}
    @assert maxDist_used<=maxDist_considered "Maximum distance of considered actuators must be grater than the maximum distance for used actuators"

    # Generate the actList_used - list of used actuators
    actList = Array{Tuple{U,U},1}();
    # Initialize the list of indices of actuators within maxDist_used
    act_used = Array{U,1}();

    # Maximum number of neighboring actuators in one direction
    n_adjActs = U(ceil(maxDist_considered/aa.dx));

    # Find the closest actuator to the position of the object
    ic, jc = closestActuator(aa, objPos[1:2]);

    ii_max = (ic + n_adjActs) > aa.nx ? aa.nx : ic + n_adjActs;
    ii_min = n_adjActs >= ic ? U(1) : ic - n_adjActs;
    jj_max = (jc + n_adjActs) > aa.ny ? aa.ny : jc + n_adjActs;
    jj_min = n_adjActs >= jc ? U(1) : jc - n_adjActs;

    # println("ii_max = $ii_max, ii_min = $ii_min, jj_max = $jj_max, jj_min = $jj_min")

    for ii in ii_min:ii_max, jj in jj_min:jj_max
        # if the actuator is in the ignore list, continue the next actuator
        if in((ii, jj), aa.ignore_list)
            continue;
        end

        # Check whether the actuator is within the specified distance from the the actuator. If it is, add the
        # actuator to the corresponding list of actuators.
        act_px, act_py = actuatorPosition(aa, (ii, jj));
        obj_px, obj_py = objPos;
        mut_dist = (obj_px-act_px)^2 + (obj_py-act_py)^2;
        if  mut_dist <= maxDist_considered^2
            push!(actList, (ii, jj));

            if mut_dist <= maxDist_used^2
                push!(act_used, length(actList));
            end
        end
    end

    actList, act_used
end

function actuatorsInCommon(al1::Array{Tuple{U, U}, 1}, al2::Array{Tuple{U, U}, 1}) where U<:Unsigned
    # Iterate over the actuators of the first and second object agent
    il1 = U[];
    il2 = U[];
    for (k, ac1) in enumerate(al1), (l, ac2) in enumerate(al2)
        if ac1 == ac2
            push!(il1, k);
            push!(il2, l);
        end
    end

    return il1, il2;
end

end


text:
@testset "scratchspace" begin
    n = 50
    p = 10
    R.allocate(n, p)
    x1 = R.SCRATCH_N[]
    x2 = R.SCRATCH_N2[]
    x3 = R.SCRATCH_N3[]
    w  = R.SCRATCH_P[]
    for v in (x1, x2, x3)
        @test v isa Vector{Float64}
        @test length(v) == n
    end
    @test w isa Vector{Float64}
    @test length(w) == p
    R.deallocate()
    x1 = R.SCRATCH_N[]
    x2 = R.SCRATCH_N2[]
    x3 = R.SCRATCH_N3[]
    w  = R.SCRATCH_P[]
    for v in (x1, x2, x3, w)
        @test length(v) == 0
    end
    n, p, c = 50, 10, 3
    R.allocate(n, p, c)
    m1 = R.SCRATCH_NC[]
    m2 = R.SCRATCH_NC2[]
    for m in (m1, m2)
        @test m isa Matrix{Float64}
        @test size(m) == (n, c)
    end
    R.deallocate()
    @test size(R.SCRATCH_NC[]) == size(R.SCRATCH_NC2[]) == (0,0)
end


text:
using Base.Test
import ArchGDAL; const AG = ArchGDAL

@testset "Testing GDAL Type Methods" begin
    @testset "GDAL Open Flags" begin
        @test AG.OF_ReadOnly | 0x04 == 0x04
        @test 0x06 | AG.OF_ReadOnly == 0x06
        @test AG.OF_ReadOnly | AG.OF_GNM == GDAL.GDAL_OF_READONLY | GDAL.GDAL_OF_GNM
    end

    @testset "GDAL Data Types" begin
        @test AG.typesize(GDAL.GDT_UInt16) == 16
        @test AG.typename(GDAL.GDT_UInt16) == "UInt16"
        @test AG.gettype("UInt16") == GDAL.GDT_UInt16

        @test AG.typeunion(GDAL.GDT_UInt16, GDAL.GDT_Byte) == GDAL.GDT_UInt16
        @test AG.iscomplex(GDAL.GDT_Float32) == false
    end

    @testset "GDAL Colors and Palettes" begin
        @test AG.getname(GDAL.GARIO_COMPLETE) == "COMPLETE"
        @test AG.asyncstatustype("COMPLETE") == GDAL.GARIO_COMPLETE
        @test AG.asyncstatustype("ERROR") == GDAL.GARIO_ERROR
        @test AG.asyncstatustype("PENDING") == GDAL.GARIO_ERROR
        @test AG.asyncstatustype("UPDATE") == GDAL.GARIO_UPDATE

        @test AG.colorinterp("Hue") == GDAL.GCI_HueBand
        @test AG.colorinterp("Red") == GDAL.GCI_RedBand
        @test AG.colorinterp("Blue") == GDAL.GCI_BlueBand

        @test AG.getname(GDAL.GPI_Gray) == "Gray"
        @test AG.getname(GDAL.GPI_RGB) == "RGB"
        @test AG.getname(GDAL.GPI_CMYK) == "CMYK"
        @test AG.getname(GDAL.GPI_HLS) == "HLS"
    end

    @testset "GDAL Field Types" begin
        @test AG.getname(GDAL.OFTString) == "String"
        @test AG.getname(GDAL.OFTIntegerList) == "IntegerList"
        @test AG.getname(GDAL.OFSTBoolean) == "Boolean"
        @test AG.getname(GDAL.OFSTFloat32) == "Float32"

        @test AG.arecompatible(GDAL.OFTReal, GDAL.OFSTNone) == true
        @test AG.arecompatible(GDAL.OFTReal, GDAL.OFSTBoolean) == false
        @test AG.arecompatible(GDAL.OFTReal, GDAL.OFSTInt16) == false
        @test AG.arecompatible(GDAL.OFTReal, GDAL.OFSTFloat32) == true
    end
end


text:
using Dynesty
using Documenter

DocMeta.setdocmeta!(Dynesty, :DocTestSetup, :(using Dynesty); recursive=true)

makedocs(;
    modules=[Dynesty],
    authors="Paul Tiede <ptiede91@gmail.com> and contributors",
    repo="https://github.com/ptiede/Dynesty.jl/blob/{commit}{path}#{line}",
    sitename="Dynesty.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        canonical="https://ptiede.github.io/Dynesty.jl",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
    ],
)

deploydocs(;
    repo="github.com/ptiede/Dynesty.jl",
    devbranch="main",
)


text:
## from Markdown.jl

import Base: display, show

graph_types = AbstractString["Plot", "FramedPlot"]

function tohtml(io::IO, m::MIME"text/html", x)
  show(io, m, x)
end

function tohtml(io::IO, m::MIME"text/latex", x)
  show(io, m, x)
end

function tohtml(io::IO, m::MIME"text/plain", x)
  show(io, m, x)
end

function tohtml(io::IO, m::MIME"image/png", img)
  print(io, """")
end

function tohtml(m::MIME"image/svg+xml", img)
    sprint(io -> show(io, m, img))
end

# Display infrastructure

function bestmime(val)
  for mime in ("text/html",  "text/latex", "application/x-latex", "image/svg+xml", "image/png", "text/plain")
    showable(mime, val) && return MIME(Symbol(mime))
  end
  error("Cannot render $val to Markdown.")
end

tohtml(io::IO, x) = tohtml(io, bestmime(x), x)

##################################################
## write mime methods

function with_environment(f, io, tag)
  print(io, "\\begin{$tag}")
  f()
  print(io, "\\end{$tag}")
end

function with_delimiter(f, io, tag)
  print(io, "$tag")
  f()
  print(io, "$tag")
end

#XXXshow(io::IO, ::MIME"text/latex", md::Markdown.Content) =
#  show(io, "text/plain", md)

##XXX function show(io::IO, mime::MIME"text/latex", block::Markdown.Block)
##   for md in block.content[1:end-1]
##     show(io::IO, mime, md)
##     println(io)
##   end
##   show(io::IO, mime, block.content[end])
## end

function show(io::IO, mime::MIME"text/latex", header::Markdown.Header{l}) where {l}
    txt = join(header.text)
    if l == 1
        print(io, "\\section{$(txt)}")
    end
    if l == 2
        print(io, "\\subsection{$(txt)}")
    end
    if l > 2
        print(io, "\\subsubsection{$(txt)}")
    end
end

"heuristic to identify code blocks"
const block_code_re = r"^\n.*\n$"
is_blockcode(content) = isa(content, Markdown.Code) && occursin(block_code_re, content.code)
#function show(io::IO, ::MIME"text/latex", code::Markdown.BlockCode)
function show(io::IO, ::MIME"text/latex", code::Markdown.Code)
println((code.code, is_blockcode(code)))
  if is_blockcode(code)
    with_delimiter(io, "verbatim") do
      print(io, code.code)
    end
  else
    txt = code.code
    txt = replace(txt, "^" => "\\^{}")
    txt = replace(txt, "_" => "\\_{}")
    txt = replace(txt, "#" => "\\#")
    txt = replace(txt, "@" => "\\@")
    print(io, "\\texttt{$(txt)}")
  end
end

#function show(io::IO, ::MIME"text/latex", code::Markdown.InlineCode)
#    print(io, "\\texttt{$(code.code)}")
#end

function show(io::IO, ::MIME"text/latex", md::Markdown.Paragraph)
    println(io, "\\newline")
    for md in md.content
      show(io, "text/latex", md)
    end
end

function show(io::IO, ::MIME"text/latex", md::Markdown.BlockQuote)
  with_environment(io, "quotation") do
    for item in md.content
        show(io, "text/latex", item)
    end
  end
end

function show(io::IO, ::MIME"text/latex", md::Markdown.List)
    with_environment(io, md.ordered < 0 ? "enumerate" : "itemize") do
    for item in md.items
        print(io, "\\item ")
        [show(io, "text/latex", i) for i in item]
    end
  end
end

function show(io::IO, ::MIME"text/latex", md::Markdown.HorizontalRule)
    print(io, "\\hrule")
end

# Inline elements

##XXX function show(io::IO, ::MIME"text/latex", md::Markdown.Plain)
##   print(io, md.text)
## end

function show(io::IO, ::MIME"text/latex", md::Markdown.Bold)
    print(io, "\\textbf{$(join(md.text))}")
end

function show(io::IO, ::MIME"text/latex", md::Markdown.Italic)
    print(io, "\\textit{$(join(md.text))}")
end

function show(io::IO, ::MIME"text/latex", code::Markdown.Image)
    print(io, "\\includegraphics{$(code.url)}")
end

# function show(io::IO, ::MIME"text/latex", md::Markdown.Image)
#   print(io, """""")
# end

function show(io::IO, ::MIME"text/latex", md::Markdown.Link)
    print(io, "\\href{$(md.url)}{$(join(md.text))}")
end

function show(io::IO, ::MIME"text/latex", md::Markdown.LaTeX)
  ## Hack, we use $$~ ~$$ to mark these up, so if we see ~..~ wrapping
  ## we add in ...
  txt = md.formula
  if occursin(r"^~.*", txt)
    print(io, "\n")
    show(io, "text/latex", L"$$")
    show(io, "text/latex",  txt[2:(end-1)])
    show(io, "text/latex", L"$$")
    print(io, "\n")
  else
    show(io, "text/plain", md)
  end
end

function show(io::IO, ::MIME"text/html", md::Markdown.LaTeX)
  ## Hack, we use $$~ ~$$ to mark these up, so if we see ~..~ wrapping
  ## we add in ...
  txt = md.formula
  if occursin(r"^~.*", txt)
    print(io, "\n")
    show(io, "text/latex", L"$$")
    show(io, "text/latex",  txt[2:(end-1)])
    show(io, "text/latex", L"$$")
    print(io, "\n")
  else
    show(io, "text/plain", md)
  end
end

function show(io::IO, ::MIME"text/latex", md::T) where {T <: AbstractString}
   print(io, md)
end

function Base.show(io::IO, ::MIME"text/latex", x::Alert)
print(io, "XXX")
end


text:
# Use baremodule to shave off a few KB from the serialized `.ji` file
baremodule TopOpt_in_PETSc_jll
using Base
using Base: UUID
import JLLWrappers

JLLWrappers.@generate_main_file_header("TopOpt_in_PETSc")
JLLWrappers.@generate_main_file("TopOpt_in_PETSc", UUID("9e4902f8-fb1c-5527-87bf-6bafcfd196f3"))
end  # module TopOpt_in_PETSc_jll


text:
using Gadfly, RDatasets, Dates

set_default_plot_size(6inch, 3inch)

y = 1973
airquality = dataset("datasets", "airquality")

dates = Date[Date(1973, m, d)
             for (m, d) in zip(airquality.Month, airquality.Day)]
airquality.Date = dates

p = plot(airquality, x=:Date, y=:Temp, Geom.line)


text:
using LinearAlgebra
using StaticArrays
using Distributions
using PyPlot
using TimerOutputs
const Vec2f = SVector{2, Float64}
const Vec4f = SVector{4, Float64}
const VecFrame = SVector{18, Float64}

include("src/geometry/geometry.jl")
include("src/world.jl")
include("src/double_integrator.jl")
include("src/fmt_cvae.jl")
include("read_scenario.jl")
include("predict.jl")
include("PyCallModel.jl")
include("PyCallPredict.jl")

lane1_c = 7.231027419583393
lane2_c = 18.84918099161813
lane3_c = 29.990037979406054
lane4_c = 40.99582664476257
lane5_c = 53.099915981198585
lane_c = [7.231027419583393, 18.84918099161813, 29.990037979406054,
          40.99582664476257, 53.099915981198585]
lane12 = (lane1_c + lane2_c)/2
lane23 = (lane2_c + lane3_c)/2
lane34 = (lane3_c + lane4_c)/2
lane45 = (lane4_c + lane5_c)/2
lane5edge = lane5_c + (lane5_c - lane45)

#us101_1_cluster.7 no case 35
#us101_1_cluster.9 no case 28
#us101_3_cluster.9 no case 3

#us101_2_cluster.9 case 45 invalid sample = 500

#us101_2_cluster.9 case 7 not easy to reach sample = 100
#us101_2_cluster.9 case 28, 38 not easy to reach sample = 100
#us101_2_cluster.9 case 45 not reach sample = 100
#us101_2_cluster.10 case 30 not easy to reach sample = 100

#us101_3_cluster.12 case 11 not easy to reach sample = 100

case_id = 10
case_name = "./us101_3/cluster_11/test_case_file"*string(case_id)*".csv"
host_file_name = "./us101_3/cluster_11/test_host_file"*string(case_id)*".csv"
global scenario, start_frame = read_scenario(case_name)
global lane1_c, lane2_c, lane3_c, lane4_c, lane5_c, lane12, lane23, lane34, lane45, lane5edge

global init_goal = get_init_goal(host_file_name)

global s_init = init_goal[1]
global s_goal = init_goal[4]
global n_goal = 2

frame_id = 3
computing_time = 0.023
curr_t = 0
global lane_id = 0
global left_prob = 0
global keep_prob = 0
global right_prob = 0

result_f = open("./time_lag_exp/test.txt", "a+")
this_traj = open("./time_lag_exp/waypoints"*string(frame_id)*".txt", "a+")
last_traj_file = "./time_lag_exp/waypoints"*string(frame_id-3)*".txt"
reset_timer!()

println("frame id:", frame_id)
global to
global start_frame
global s_goal
global isOK = true
global new_start = false
if frame_id == 0
    s_init = init_goal[1]
    s_goal = init_goal[4]
    if 0 < s_init[1] < lane12
        global lane_id = 1
    elseif lane12 < s_init[1] < lane23
        global lane_id = 2
    elseif lane23 < s_init[1] < lane34
        global lane_id = 3
    elseif lane34 < s_init[1] < lane45
        global lane_id = 4
    elseif lane45 < s_init[1] < lane5edge
        global lane_id = 5
    end
    # println(length(frame))
    global prev_frame = get_frame(scenario, frame_id-1, start_frame)
    global frame1 = get_frame(scenario, frame_id, start_frame)
    #global frame2, frame3, frame4, frame5, frame6, frame7, frame8, frame9, frame10 = predict_action(prev_frame, frame1, frame_id+start_frame)
    global frame2 = get_frame(scenario, frame_id+1, start_frame)
    global frame3 = get_frame(scenario, frame_id+2, start_frame)
    global frame4 = get_frame(scenario, frame_id+3, start_frame)
    global frame5 = get_frame(scenario, frame_id+4, start_frame)
    global frame6 = get_frame(scenario, frame_id+5, start_frame)
    global frame7 = get_frame(scenario, frame_id+6, start_frame)
    global frame8 = get_frame(scenario, frame_id+7, start_frame)
    global frame9 = get_frame(scenario, frame_id+8, start_frame)
    global frame10 = get_frame(scenario, frame_id+9, start_frame)

    center = Vec2f(s_init[1], s_init[2])
    # println(length(frame))
    global neighbors1, front_neighbors1 = find_neighbors(frame1, center, lane_id, 150.0)
    global neighbors2, front_neighbors2 = find_neighbors(frame2, center, lane_id, 150.0)
    global neighbors3, front_neighbors3 = find_neighbors(frame3, center, lane_id, 150.0)
    global neighbors4, front_neighbors4 = find_neighbors(frame4, center, lane_id, 150.0)
    global neighbors5, front_neighbors5 = find_neighbors(frame5, center, lane_id, 150.0)
    global neighbors6, front_neighbors6 = find_neighbors(frame6, center, lane_id, 150.0)
    global neighbors7, front_neighbors7 = find_neighbors(frame7, center, lane_id, 150.0)
    global neighbors8, front_neighbors8 = find_neighbors(frame8, center, lane_id, 150.0)
    global neighbors9, front_neighbors9 = find_neighbors(frame9, center, lane_id, 150.0)
    global neighbors10, front_neighbors10 = find_neighbors(frame10, center, lane_id, 150.0)
    # println(length(neighbors))
    obstacle_set = set_obstacles(neighbors1)
else
    cum_time = Float64[]
    cum_time_f = open("./time_lag_exp/cum_time0.txt","r")
    n = countlines(cum_time_f)
    seekstart( cum_time_f )
    println(n)
    for i = 1:n
        line = readline(cum_time_f)
        if line != ""
            push!(cum_time, parse(Float64, line))
        end
    end
    waypoints = Vec4f[]
    waypoints_f = open(last_traj_file,"r")
    n = countlines(waypoints_f)
    seekstart( waypoints_f )
    for i = 1:n
        line = split( readline( waypoints_f ), "," )
        if line[1] == ""
            continue
        end
        p = Vec4f(parse(Float64, line[1]),
                parse(Float64, line[2]),
                parse(Float64, line[3]),
                parse(Float64, line[4]))
        push!(waypoints, p)
    end
    global traj, future_wy, reach = get_next_position(cum_time, waypoints, 0.3+computing_time, s_goal)
    if !reach
        trajvalid1 = check_traj(cum_time, waypoints, 0.1, frame_id+1, s_goal, scenario, start_frame)
        trajvalid2 = check_traj(cum_time, waypoints, 0.2, frame_id+2, s_goal, scenario, start_frame)
        trajvalid3 = check_traj(cum_time, waypoints, 0.3, frame_id+3, s_goal, scenario, start_frame)

        if !trajvalid1
            println("traj1 invalid")
        end
        if !trajvalid2
            println("traj2 invalid")
        end
        if !trajvalid3
            println("traj3 invalid")
        end
    end

    if reach
        println("congratulations!!!successful！！！")
        for p in traj
            write(result_f, string(p[1]))
            write(result_f, ',')
            write(result_f, string(p[2]))
            write(result_f, ',')
            write(result_f, string(p[3]))
            write(result_f, ',')
            write(result_f, string(p[4]))
            write(result_f, '\n')
        end
        print_timer()
    end
    global s_init = traj[end]
    print("init:", s_init)
    print("goal:", s_goal)
    println("goal distance:", sqrt((s_init[1]-s_goal[1])^2+(s_init[2]-s_goal[2])^2))
    if sqrt((s_init[1]-s_goal[1])^2+(s_init[2]-s_goal[2])^2+(s_init[3]-s_goal[3])^2+(s_init[4]-s_goal[4])^2) < 10
        println("congratulations!!!successful！！！")
        exit()
    end
    global prev_frame = get_frame(scenario, frame_id-1, start_frame)
    global frame1 = get_frame(scenario, frame_id, start_frame)
    # global frame2, frame3, frame4, frame5, frame6, frame7, frame8, frame9, frame10 = predict_action(prev_frame, frame1, frame_id+start_frame)
    global frame2 = get_frame(scenario, frame_id+1, start_frame)
    global frame3 = get_frame(scenario, frame_id+2, start_frame)
    global frame4 = get_frame(scenario, frame_id+3, start_frame)
    global frame5 = get_frame(scenario, frame_id+4, start_frame)
    global frame6 = get_frame(scenario, frame_id+5, start_frame)
    global frame7 = get_frame(scenario, frame_id+6, start_frame)
    global frame8 = get_frame(scenario, frame_id+7, start_frame)
    global frame9 = get_frame(scenario, frame_id+8, start_frame)
    global frame10 = get_frame(scenario, frame_id+9, start_frame)

    global center = Vec2f(s_init[1], s_init[2])
    if 0 < s_init[1] < lane12
        global lane_id = 1
    elseif lane12 < s_init[1] < lane23
        global lane_id = 2
    elseif lane23 < s_init[1] < lane34
        global lane_id = 3
    elseif lane34 < s_init[1] < lane45
        global lane_id = 4
    elseif lane45 < s_init[1] < lane5edge
        global lane_id = 5
    end
    global neighbors1, front_neighbors1 = find_neighbors(frame1, center, lane_id, 150.0)
    global neighbors2, front_neighbors2 = find_neighbors(frame2, center, lane_id, 150.0)
    global neighbors3, front_neighbors3 = find_neighbors(frame3, center, lane_id, 150.0)
    global neighbors4, front_neighbors4 = find_neighbors(frame4, center, lane_id, 150.0)
    global neighbors5, front_neighbors5 = find_neighbors(frame5, center, lane_id, 150.0)
    global neighbors6, front_neighbors6 = find_neighbors(frame6, center, lane_id, 150.0)
    global neighbors7, front_neighbors7 = find_neighbors(frame7, center, lane_id, 150.0)
    global neighbors8, front_neighbors8 = find_neighbors(frame8, center, lane_id, 150.0)
    global neighbors9, front_neighbors9 = find_neighbors(frame9, center, lane_id, 150.0)
    global neighbors10, front_neighbors10 = find_neighbors(frame10, center, lane_id, 150.0)
    global obstacle_set = set_obstacles(neighbors1)
    global x_min, x_max, v_min, v_max = get_boundary(neighbors1, center, lane_id, s_goal)
    global W = World(x_min, x_max, v_min, v_max, obstacle_set)

    for p in traj
        write(result_f, string(p[1]))
        write(result_f, ',')
        write(result_f, string(p[2]))
        write(result_f, ',')
        write(result_f, string(p[3]))
        write(result_f, ',')
        write(result_f, string(p[4]))
        write(result_f, '\n')
    end
end

x_min, x_max, v_min, v_max = get_boundary(neighbors1, center, lane_id, s_goal)
W = World(x_min, x_max, v_min, v_max, obstacle_set)
# show(W)
# scatter(s_init[1], s_init[2])
# scatter(s_goal[1], s_goal[2])
# savefig("./us101_3_cluster_100/cluster10/test_0_world.png")
# exit()

Nsample = 110
right_prob = 1/3
left_prob = 1/3
keep_prob = 1/3
println("init:", s_init)
println("goal:", s_goal)
frames = [frame1, frame2, frame3, frame4, frame5, frame6, frame7, frame8, frame9, frame10]
fmt = @time FMTree(s_init, s_goal, Nsample, W, right_prob, left_prob, keep_prob, lane_id, frames)
println(length(fmt.Pset))
with_savefig = false
# println("fmt.s_init:", fmt.s_init)
@timeit "solve "*string(frame_id) idx_solution = solve(fmt, frame_id, with_savefig)
clf()
show(fmt)

t = Float64[]
waypoints = Vec4f[]
for idx in idx_solution
    global waypoints
    s0 = fmt.Pset[fmt.parent[idx]]
    s1 = fmt.Pset[idx]
    tau = fmt.time[idx]
    println("s0:", s0)
    println("s1:", s1)
    # println(tau)
    points = show_trajectory(s0, s1, tau, 20, :blue, 1.5)
    if idx != 1
        waypoints = vcat(points, waypoints)
    end
    # print(waypoints)
    scatter(s0[1], s0[2], c=:black, s=10)
    # scatter(s1[1], s1[2], c=:red, s=10)
end
global waypoints
global cum_time = cal_time(waypoints)

for p in waypoints
    write(this_traj, string(p[1]))
    write(this_traj, ',')
    write(this_traj, string(p[2]))
    write(this_traj, ',')
    write(this_traj, string(p[3]))
    write(this_traj, ',')
    write(this_traj, string(p[4]))
    write(this_traj, '\n')
end

time_file = open("./time_lag_exp/cum_time"*string(frame_id)*".txt","a+")
for t in cum_time
    write(time_file, string(t))
    write(time_file, '\n')
end

savefig("./time_lag_exp/test_case_"*string(frame_id)*".png")

open("./time_lag_exp/mytime"*string(frame_id)*".txt","a+") do io
    print_timer(io, TimerOutputs.get_defaulttimer(), allocations=false)
end
exit()


text:
"""
    RegularInTime{
        Tt, Tv, Tts<:AbstractVector{Tt}, Tvs<:AbstractVector{<:AbstractVector{Tv}},
    } <: AbstractVector{Tuple{Tt, Tv}}

Represents data that has multiple observations at each of a given collection of time slices.
"""
struct RegularInTime{
    Tt, Tv, Tts<:AbstractVector{Tt}, Tvs<:AbstractVector{<:AbstractVector{Tv}},
} <: AbstractVector{Tuple{Tt, Tv}}
    ts::Tts
    vs::Tvs
end

get_space(x::RegularInTime) = Zygote.literal_getfield(x, Val(:vs))

get_times(x::RegularInTime) = Zygote.literal_getfield(x, Val(:ts))

Base.size(x::RegularInTime) = (sum(length, x.vs), )

function Base.collect(x::RegularInTime)
    time_inputs = reduce(vcat, [fill(t, length(x)) for (t, x) in zip(x.ts, x.vs)])
    space_inputs = reduce(vcat, x.vs)
    return [(x, t) for (x, t) in zip(space_inputs, time_inputs)]
end

Base.getindex(x::RegularInTime, n::Int) = collect(x)[n]

Base.show(io::IO, x::RegularInTime) = Base.show(io::IO, collect(x))

#
# Implement internal API for transforming between "flat" representation, which is useful for
# GPs, an a time-centric representation, which is useful for state-space models.
#

# See docstring elsewhere for context.
inputs_to_time_form(x::RegularInTime{<:Real}) = get_space(x)

# See docstring elsewhere for context.
function observations_to_time_form(x::RegularInTime, y::AbstractVector)
    return restructure(y, length.(get_space(x)))
end

# See docstring elsewhere for context.
function noise_var_to_time_form(x::RegularInTime, S::Diagonal{<:Real})
    vs = restructure(S.diag, length.(get_space(x)))
    return Diagonal.(collect.(vs))
end

destructure(::RegularInTime, y::AbstractVector{<:AbstractVector{<:Real}}) = reduce(vcat, y)

function restructure(y::AbstractVector{<:Real}, lengths::AbstractVector{<:Integer})
    idxs_start = cumsum(vcat(0, lengths)) .+ 1
    idxs_end = idxs_start[1:end-1] .+ lengths .- 1
    return map(n -> y[idxs_start[n]:idxs_end[n]], eachindex(lengths))
end

function restructure(y::AbstractVector{T}, lengths::AbstractVector{<:Integer}) where {T}
    idxs_start = cumsum(vcat(0, lengths)) .+ 1
    idxs_end = idxs_start[1:end-1] .+ lengths .- 1
    return map(eachindex(lengths)) do n
        y_missing = Vector{T}(undef, lengths[n])
        y_missing .= y[idxs_start[n]:idxs_end[n]]
        return y_missing
    end
end

function Zygote._pullback(
    ::AContext, ::typeof(restructure), y::Vector, lengths::AbstractVector{<:Integer},
)
    restructure_pullback(Δ::Vector) = nothing, reduce(vcat, Δ), nothing
    return restructure(y, lengths), restructure_pullback
end

# Implementation specific to Fills for AD's sake.
function restructure(y::Fill{<:Real}, lengths::AbstractVector{<:Integer})
    return map(l -> Fill(y.value, l), Zygote.dropgrad(lengths))
end

function restructure(y::AbstractVector, emissions::StructArray)
    return restructure(y, Zygote.dropgrad(map(dim_out, emissions)))
end


text:
using NLOptControl
using VehicleModels
using Parameters
using DataFrames
using MAVs
using PrettyPlots,Plots
pgfplots()

c=defineCase(;(:mode=>:autoBench));

n=initializeAutonomousControl(c);

t_optJ=zeros(10);
t_optJ[1]=n.r.t_solve

for i in 2:10
  optimize!(n);
  t_optJ[i]=n.r.t_solve
end
aveJ=mean(t_optJ[2:end])

# extract data from Jiechao's tests
dfs=dataSet(:jiechao_benchmark,"Jiechao_results/")
x=dfs[1][:x];y=dfs[1][:y]
t_optM=[1.5401168711,0.6988501533,0.6917544356,0.7267272741,0.7018273181,0.6902223212,0.7143454254,0.6686120402,0.6768629488,0.6893707926];
aveM=mean(t_optM[2:end])

# plot the results
results_dir=string("benchmark1_",c.m.name,"/")
resultsDir!(n;results_name=results_dir);

# optimization times
optP=scatter(1:10,t_optJ,label="julia")
scatter!(1:10,t_optM,label="MATLAB")
yaxis!("Optimization Time (s)");
xaxis!("Evaluation Number");
title!(string("MATLAB ave. = ",round(aveM,2) ,"julia ave. = ",round(aveJ,2) ))
savefig(string(n.r.results_dir,"/","optPlot",".",:svg));

# position
pos=obstaclePlot(n,c)
plot!(n.r.X[:,1],n.r.X[:,2],label="julia")
plot!(x,y,label="MATLAB")
xaxis!(n.state.description[1]);
yaxis!(n.state.description[2]);
xlims!((200-55,200+55))
ylims!((-1,109))
savefig(string(n.r.results_dir,"/","position",".",:svg));


text:
using ConstrainedDynamics
using ConstrainedDynamicsVis
using ConstrainedDynamics: GenericJoint,Vmat,params
using StaticArrays

# Parameters
length1 = 1.0
width, depth = 0.1, 0.1

# Links
origin = Origin{Float64}()
link1 = Box(width, depth, length1, length1)

@inline function g(joint::GenericJoint, xb::AbstractVector, qb::UnitQuaternion)
    a=5
    b=2

    if xb[3]==0
        xb[2] > 0 ? α = -pi/2 : α = pi/2
    elseif xb[3] > 0
        α = -atan(b^2/a^2*xb[2]/xb[3])
    else
        α = -atan(b^2/a^2*xb[2]/xb[3]) - pi
    end

    qα = UnitQuaternion(cos(α/2),sin(α/2),0,0,false)

    eqc1=Vmat(qb\qα)
    eqc2=SA[xb[1]; (xb[2]^2/a^2)+(xb[3]^2/b^2)-1]
    G= [eqc1;eqc2]
    return G
end

# Constraints
joint_between_origin_and_link1 = EqualityConstraint(GenericJoint{5}(origin,link1,g))

links = [link1]
constraints = [joint_between_origin_and_link1]

mech = Mechanism(origin, links, constraints)
setPosition!(link1,x=[0;0.0;2],q = UnitQuaternion(RotX(0)))
setVelocity!(link1,v=[0;2.0;0])

steps = Base.OneTo(1000)
storage = Storage{Float64}(steps,1)

simulate!(mech, storage, record = true)
visualize(mech, storage)


text:
# This file is a part of EasyPkg, licensed under the MIT License (MIT).

include_sources(
    "find_sources.jl",
    "include_all_sources.jl",
)


text:
function circular_traj(dy::CarDynamics, x0, u0, t_end::𝕋)::Vector{CarX{ℝ}}
    x::CarX{ℝ} = x0
    out = Vector{CarX{ℝ}}(undef, t_end)
    for t in 1:t_end
        out[t] = x
        x = sys_forward(dy, x, u0, t)
    end
    out
end

function plot_tracking(data::TrajectoryData, freq::ℝ, ref_traj::Vector{CarX{ℝ}})
    scene, layout = layoutscene(resolution = (1600, 1600))

    xs, ys = data["x"][:,1], data["y"][:,1]
    θs = data["θ"][:,1]

    ax_traj = layout[1,1] = LAxis(
        scene, title = "Trajectories", aspect = DataAspect(),
        backgroundcolor = RGBf0(0.98, 0.98, 0.98))
    ref_plot = let
        local xs = (p -> p.x).(ref_traj)
        local ys = (p -> p.y).(ref_traj)
        lines!(ax_traj, xs, ys; color = :green, linewidth = 4)
    end
    traj_plot = lines!(ax_traj, xs, ys; linewidth = 4)

    times = data.times
    time_ls = labelslider!(
    scene, "time", times;
    format = x -> "$(round((x - 1) / freq, digits = 2))s")
    layout[2,1] = time_ls.layout

    t = time_ls.slider.value
    car = @lift car_triangle(xs[$t], ys[$t], θs[$t])
    ref_car = @lift let c = ref_traj[$t]
        car_triangle(c.x, c.y, c.θ)
    end
    poly!(ax_traj, car; color = :red)
    poly!(ax_traj, ref_car; color = :green)

    scene
end

function tracking_example(;setting::ExampleSetting, plot_result = true)
    X, Z, U = CarX{ℝ}, SVector{0, ℝ}, CarU{ℝ}
    @unpack time_end, freq, noise, H, delays = setting
    t_end = 𝕋(ceil(time_end * freq))
    delta_t = 1 / freq

    # Car dynamics parameters
    dy = CarDynamics(;delta_t, max_ψ = 60°)
    rng = MersenneTwister(1234)
    function add_noise(x::X, t)::X where X
        x + randn(rng, X) * noise
    end
    dy_actual = @set dy.add_noise = add_noise
    z_dy = StaticObsDynamics()

    @unpack ΔT, Ta = short_delay_names(delays)

    # reference trajectory to track
    x_ref0 = X(x = 0, y = -0.5, θ = 0.0)
    u_ref0 = U(v̂ = 0.5, ψ̂ = 0.1pi)
    circ_traj = circular_traj(dy, x_ref0, u_ref0, t_end + 1 + H * ΔT + Ta)
    traj_f = FuncT(Tuple{ℕ,𝕋}, CarX{ℝ}) do (id, t)
        t = max(1, t)
        circ_traj[t]
    end

    RefK = RefPointTrackControl(;dy, ref_pos = dy.l, ctrl_interval = ΔT * delta_t, kp = 1.0, ki = 1.0)
    # RefK = ConfigTrackControl(;dy, k1 = 2, k2 = 1, k3 = 1)
    central = RefTrackCentralControl(RefK, traj_f)

    init = let x0 = X(x = 0, y = 0, θ = 180°), z0 = Z(), u0 = U()
        [(x0, z0, u0)]
    end

    N = 1
    world_model = WorldDynamics([(dy, z_dy)])
    world_actual = WorldDynamics([(dy_actual, z_dy)])
    # framework = NaiveCF{X,Z,U}(N, central, delay_model.com)
    framework = let
        world_model = world_model
        x_weights = fill(X(x = 1, y = 1, θ = 1), N)
        u_weights = fill(U(v̂ = 1, ψ̂ = 1), N)
        OvCF(central, world_model, delays, x_weights, u_weights; X, Z, N, H)
    end

    comps = ["x", "y", "θ", "ψ"]
    function record_f(xs, zs, us)
        [(x -> x.x).(xs) (x -> x.y).(xs) (x -> x.θ).(xs) (x -> x.ψ).(xs)]
    end

    result, logs = simulate(
        world_actual, delays, framework, init, (comps, record_f), 1:t_end)
    # visualize(result; delta_t = 1 / freq) |> display
    if plot_result
        plot_tracking(result, freq, circ_traj) |> display
    end
    result, logs
end


text:
using Compat.Test
import Roots.newton, Roots.halley

@test norm(newton(sin, cos, 0.5) - 0.0) <= 100*eps(1.0)
@test newton(cos, x -> -sin(x), 1.0)  ≈ pi/2
@test newton(x -> x^2 - 2x - 1, x -> 2x - 2, 3.0)  ≈ 2.414213562373095
@test norm(newton(x -> exp(x) - cos(x), x -> exp(x) + sin(x), 3.0) - 0.0) <= 1e-14
@test halley(x -> x^2 - 2x - 1,x -> 2x - 2,x -> 2, 3.0)  ≈ 2.414213562373095
a = halley(x -> exp(x) - cos(x),
           x -> exp(x) + sin(x),
           x -> exp(x) + cos(x), 3.0)
@test norm(a - 0.0) <= 1e-14

## tests with auto derivatives
@test newton(sin, 3) ≈ pi
@test halley(sin, 3) ≈ pi

## More tests with autoderivaitves. Derivative of D operation:
isdefined(ForwardDiff, :derivative) && @test newton(D(sin), 1.5) ≈ pi/2

## test with Complex input

@test real(newton(x ->  x^3 - 1, x ->  3x^2, 1+im)) ≈ 1.0
@test real(newton(x ->  x^3 - 1, x ->  3x^2, 1+10im)) ≈ (-1/2)