using Devectorize
using Clustering
# using Debug
include("levenberg_marquardt.jl")
include("../cuckooSearch.jl")

type RbfNonlinearParameters
    x::Vector{Float64}
    n_state::Integer
    n_centers::Integer
end


# for op = (:+, :*, :\, :/)
#     @eval ($op)(a::RbfNonlinearParameters,b) = ($op)(a.x,b)
#     @eval ($op)(b,a::RbfNonlinearParameters) = ($op)(b,a.x)
# end

Base.display(p::RbfNonlinearParameters) = println("RbfNonlinearParameters: Parameters = $(length(p.x)), n_state(x) = $(p.n_state), n_centers(x) = $(p.n_centers)")
Base.start(p::RbfNonlinearParameters) =  1
Base.done(p::RbfNonlinearParameters, state) = state > p.n_centers
Base.next(p::RbfNonlinearParameters, state) = (p.x[1+(state-1)*2*p.n_state:state*2p.n_state])::VecOrMat, state + 1
"""Train your RBF-ARX network.`trainRBF_ARX(y, na, nc; state = :A, liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, outputnet = true, cuckoosearch = false)`\n
The number of centers is equal to `nc` if Kmeans is used to get initial centers, otherwise the number of centers is `nc^n_state`\n
`n_state` is equal to the state dimension, possibly reduced to `inputpca` if so desired.\n
The number of nonlinear parameters is `n_centers × n_state`\n
The number of linear parameters is `outputnet ? n_state × (n_centers+1) × (na)+1) : (na)×(n_centers+1)+1)`"""
function trainRBF_ARX(y, A, state, nc; liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, outputnet = true, cuckoosearch = false, cuckooiter=100, timeout=60)
    n_points = length(y)
    na = isa(A,Matrix) ? size(A,2) : 1

    function predictionerror(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        psi = getΨ(Ψ, znl, state, n_points, n_state, normalized)
        getLinearRegressor(V,psi,A,state,outputnet,na,n_state,n_centers,n_points)
        zl = fitlinear(V,y);
        prediction = V*zl
        error = prediction-y
        return error
    end

    # Get initial centers ================================
    Znl::RbfNonlinearParameters
    state,n_state, TT = inputtransform(state, inputpca)

    if lowercase(initialcenters) == "equidistant"
        initialcenters = :equidistant
        n_centers = nc^n_state
    else
        initialcenters = :kmeans
        n_centers = nc
    end

    Ψ = Array(Float64,(n_points,n_centers+1))
    Ψ[:,end] = 1.0
    V = outputnet ? V = Array(Float64,(n_points, n_state* (n_centers+1)* (na)+1)) : V = Array(Float64,(n_points, (na)*(n_centers+1)+1))
    V[:,end] = 1.0
    if initialcenters == :equidistant
        Znl = getcentersEq(state,nc); debug("gotcentersEq")
    else
        Znl = getcentersKmeans(state, nc, predictionerror, n_state); debug("gotcentersKmeans")
    end
    getΨ(Ψ, Znl, state, n_points, n_state, normalized); debug("Got Ψ")
    @ddshow sum(!isfinite(Ψ))
    getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points); debug("Got linear regressor V")
    @ddshow size(V)
    @ddshow sum(!isfinite(V))
    Zl = fitlinear(V,y); debug("fitlinear")
    @ddshow sum(!isfinite(Zl))
    prediction = V*Zl
    error = y - prediction
    errors = zeros(liniters+1)
    Lb,Ub = getbounds(Znl, state, n_state, n_centers)

    # ============= Main loop  ================================================
    debug("Calculating initial error")
    errors[1] = rms(predictionerror(Znl.x))
    println("Training RBF_ARX Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl))")
    function g(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        w = fitlinear(V,y)
        return outputnet ? jacobian_outputnet(znl,Ψ, w, V, state) : jacobian_no_outputnet(znl,Ψ, w, V, state)
    end
    function grad(z,range)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        w = fitlinear(V,y)
        return outputnet ? gradient_outputnet(znl,Ψ, V, state,A,y,w, range) : gradient_no_outputnet(znl,Ψ, V, state,A,y,w, range)
    end
    f(z) = predictionerror(z)
    X0 = deepcopy(Znl.x)
    for i = 1:liniters
        if i%2 == 1 || !cuckoosearch
            @time res = levenberg_marquardt(f, g, X0,
                                            maxIter = nonliniters,
                                            tolG = 1e-7,
                                            tolX = 1e-10,
                                            show_trace=true,
                                            timeout = timeout,
                                            n_state = n_state)
            #             @time res = stochastic_gradient_descent(f, grad, X0, n_points, n_state,
            #                                                     lambda = 0.001,
            #                                                     maxIter = 5,
            #                                                     tolG = 1e-7,
            #                                                     tolX = 1e-10,
            #                                                     show_trace=true,
            #                                                     timeout = 1000,
            #                                                     batch_size=50)
            X0 = deepcopy(res.minimum)
            DEBUG && assert(X0 == res.minimum)
            DEBUG && @show ff1 = rms(f(X0))
            if DEBUG
                _V = deepcopy(V)
                _Ψ = deepcopy(Ψ)
            end
            DEBUG && @show ff2 = rms(f(res.minimum))

            if DEBUG
                @assert ff1 == ff2
                @show res.minimum == X0
                @show _V == V
                @show _Ψ == Ψ
            end
            assert(X0 == res.minimum)
            #             Znl = RbfNonlinearParameters(saturatePrecision(copy(res.minimum),n_state),n_state,n_centers)
            Znl = RbfNonlinearParameters(deepcopy(res.minimum),n_state,n_centers)
            errors[i+1] = res.f_minimum
            # show(res.trace)
        else
            display("Using cuckoo search to escape local minimum")
            @time (bestnest,fmin) = cuckoo_search(x -> rms(f(x)),X0, Lb, Ub;
                                                  n=50,
                                                  pa=0.25,
                                                  Tol=1.0e-5,
                                                  max_iter = i < liniters-1 ? cuckooiter : 2cuckooiter,
                                                  timeout = 2timeout)
            debug("cuckoo_search done")
            X0 = deepcopy(bestnest)
            @ddshow rms(f(X0))
            Znl = RbfNonlinearParameters(deepcopy(bestnest),n_state,n_centers)
            errors[i+1] = fmin
        end
        if abs(errors[i+1]-errors[i]) < 1e-10
            display("No significant change in function value")
            break
        end
    end

    # Test ===============================================
    getΨ(Ψ, Znl, state, n_points, n_state, normalized)
    getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points)
    Zl = fitlinear(V,y); debug("fitlinear")
    prediction = V*Zl
    error = y - prediction
    if PYPLOT || WINSTON
        newplot(y,"k");
        plot(prediction,"b");
        plot(error,"r");title("Fitresult, RBF-ARX, n_a: $na, n_c: $(Znl.n_centers), Nonlin params: $(length(Znl.x)), Lin params: $(length(Zl)) RMSE = $(rms(error)) Fit = $(fit(y,prediction))")
        plotcenters(Znl)
        newplot(errors,"o"); title("Errors");
    end

    # Exit ===============================================
    println("tainRBF_ARX done. Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(Zl)), RMSE: $(rms(error))")
end



function trainRBF(y, state, nc; liniters=3,nonliniters=50, normalized=false, initialcenters="equidistant", inputpca=false, cuckoosearch = false, cuckooiter=100, timeout=60)
    n_points = length(y)
    function predictionerror(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        psi = getΨ(Ψ, znl, state, n_points, n_state, normalized)
        zl = fitlinear(Ψ,y);
        prediction = Ψ*zl
        error = prediction-y
        return error
    end

    # Get initial centers ================================
    Znl::RbfNonlinearParameters

    state, n_state, TT = inputtransform(state, inputpca)
    #     n_state = size(state,2)

    if lowercase(initialcenters) == "equidistant"
        initialcenters = :equidistant
        n_centers = nc^n_state
    else
        initialcenters = :kmeans
        n_centers = nc
    end
    Ψ = Array(Float64,(n_points,n_centers+1))
    Ψ[:,end] = 1.0
    if initialcenters == :equidistant
        Znl = getcentersEq(state,nc); debug("gotcentersEq")
    else
        Znl = getcentersKmeans(state, nc, predictionerror, n_state); debug("gotcentersKmeans")
    end
    #     Znl.x += 0.1*randn(size(Znl.x))
    @ddshow Znl
    getΨ(Ψ, Znl, state, n_points, n_state, normalized); debug("Got Ψ")
    #     matshow(deepcopy(Ψ));axis("tight"); colorbar()
    #     plotparams(Znl)


    @ddshow sum(!isfinite(Ψ))
    w = fitlinear(Ψ,y); debug("fitlinear")
    #     newplot(w,"o"); title("Linear parameters")
    @ddshow sum(!isfinite(Zl))
    prediction = Ψ*w
    error = y - prediction
    #     newplot([y prediction error]);title("inbetween")
    errors = zeros(liniters+1)
    Lb,Ub = getbounds(Znl, state, n_state, n_centers)

    # ============= Main loop  ================================================
    debug("Calculating initial error")
    errors[1] = rms(predictionerror(Znl.x))
    println("Training RBF_ARX Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(w))")
    function g(z)
        znl = RbfNonlinearParameters(z,n_state,n_centers)
        w = fitlinear(Ψ,y)
        return jacobian(znl,Ψ, w, state)
    end
    f(z) = predictionerror(z)
    X0 = deepcopy(Znl.x)
    for i = 1:liniters
        if i%2 == 1 || !cuckoosearch
            @time res = levenberg_marquardt(f, g, X0,
                                            maxIter = nonliniters,
                                            tolG = 1e-7,
                                            tolX = 1e-10,
                                            show_trace=true,
                                            timeout = timeout,
                                            n_state = n_state)
            X0 = deepcopy(res.minimum)
            DEBUG && assert(X0 == res.minimum)
            DEBUG && @show ff1 = rms(f(X0))
            if DEBUG
                _Ψ = deepcopy(Ψ)
            end
            DEBUG && @show ff2 = rms(f(res.minimum))

            if DEBUG
                @assert ff1 == ff2
                @show res.minimum == X0
                @show _Ψ == Ψ
            end
            assert(X0 == res.minimum)
            #             Znl = RbfNonlinearParameters(saturatePrecision(copy(res.minimum),n_state),n_state,n_centers)
            Znl = RbfNonlinearParameters(deepcopy(res.minimum),n_state,n_centers)
            errors[i+1] = res.f_minimum
            # show(res.trace)
        else
            display("Using cuckoo search to escape local minimum")
            @time (bestnest,fmin) = cuckoo_search(x -> rms(f(x)),X0, Lb, Ub;
                                                  n=50,
                                                  pa=0.25,
                                                  Tol=1.0e-5,
                                                  max_iter = i < liniters-1 ? cuckooiter : 2cuckooiter,
                                                  timeout = 2timeout)
            debug("cuckoo_search done")
            X0 = deepcopy(bestnest)
            @ddshow rms(f(X0))
            Znl = RbfNonlinearParameters(deepcopy(bestnest),n_state,n_centers)
            errors[i+1] = fmin
        end
        if abs(errors[i+1]-errors[i]) < 1e-10
            display("No significant change in function value")
            break
        end
    end

    # Test ===============================================
    getΨ(Ψ, Znl, state, n_points, n_state, normalized)
    w = fitlinear(Ψ,y); debug("fitlinear")
    prediction = Ψ*w
    error = y - prediction
    if PYPLOT || WINSTON
        newplot(y,"k");
        plot(prediction,"b");
        plot(error,"r");title("Fitresult, RBF, n_a: $na, n_c: $(Znl.n_centers), Nonlin params: $(length(Znl.x)), Lin params: $(length(w)) RMSE = $(rms(error)) Fit = $(fit(y,prediction))")
        plotcenters(Znl)
        newplot(errors,"o"); title("Errors");
    end

    # Exit ===============================================
    println("tainRBF done. Centers: $(Znl.n_centers), Nonlinear parameters: $(length(Znl.x)), Linear parameters: $(length(w)), RMSE: $(rms(error))")
    return prediction
end

type InputTransform
    mean
    variance
    C
end

function inputtransform(state, inputpca)
    n_points = size(state,1)
    m = mean(state,1)
    v = std(state,1)
    state-= repmat(m,n_points,1)
    state./= repmat(v,n_points,1)
    if isa(inputpca, Int)
        if inputpca > size(state,2)
            warn("inputpca must be <= n_state")
            inputpca = size(state,2)
        end
        C,score,latent,W0 = PCA(state,true)
        state = score[:,1:inputpca]
    else
        C = 1
    end
    n_state = size(state,2)
    #     newplot(state); title("State after transformation")
    return state, n_state, InputTransform(m,v,C)

end

function getcentersEq(state::VecOrMat, nc::Integer)
    n_points = size(state,1)
    n_state = typeof(state) <: Matrix ? size(state,2) : 1
    state_extrema = [minimum(state,1)' maximum(state,1)']
    statewidths = state_extrema[:,2] - state_extrema[:,1]
    Δc = statewidths/nc
    Z = zeros(nc, 2*n_state) # 2*n_state to fit center coordinates and scaling parameters
    # Fill initial centers
    for i = 1:n_state
        Z[:,i] = collect((state_extrema[i,1]+ Δc[i]/2):Δc[i]:state_extrema[i,2])
    end
    # add bandwidth parameters γ, give all centers the same bandwidth with Δc as a (hopefully) good initial guess
    #     display(Z)
    Z[:,n_state+1:end] = 1*repmat(4./(Δc.^2)',nc,1) # Spread the initial guess to all centers
    assert(all(Z[:,n_state+1:end].> 0))
    debug("Z done")
    n_centers::Int64 = nc^n_state # new number of centers wich considers gridding of 1D centers
    ZZ1 = zeros(n_state, n_centers)
    ZZ2 = zeros(n_state, n_centers)
    # Here comes the magic. Spread each one dimensional center onto a grid in n_state dimensions
    v = n_centers
    h = 1
    ii = 1
    for i = 1:n_state # For each iteration, v decreases and h increases by a factor of nc. If v and h are used in repmat(⋅,v,h) which is then vecotrized, the desired grid will be created. It's a bit tricky, but makes sense after a lot of thinking
        v = convert(Int64, v / nc)
        ZZ1[i,:] = vec(repmat(Z[:,i]',v,h))'
        ZZ2[i,:] = vec(repmat(Z[:,i+n_state]',v,h))'
        h *= nc
        ii += 1
    end
    debug("ZZ done")
    RbfNonlinearParameters(vec([ZZ1; ZZ2]), n_state, n_centers)
    #error("Bias parameter!")
end

function getcentersKmeans(state, nc::Int, f::Function, n_state::Int)
    iters = 21
    errorvec = zeros(iters)
    params = Array(Float64,(nc*2*n_state,iters))
    methods = [:rand;:kmpp]
    for iter = 1:iters
        clusterresult = kmeans(state', nc; maxiter=200, display=:none, init=iter<iters ? methods[iter%2+1] : :kmcen)
        for i = 1:nc
            si = 1+(i-1)n_state*2
            params[si:si+n_state-1,iter] = clusterresult.centers[:,i]
            C = cov(state[clusterresult.assignments .== i,:])
            C = cond(C) > 1e4 ? eye(C) : C
            params[si+n_state:si+2n_state-1,iter] = diag(inv(C))
            any(diag(inv(C)) .< 0) && show(C)
        end
        errorvec[iter] = rms(f(params[:,iter]))
    end
    println("Std in errors among initial centers: ", round(std(errorvec),6))
    ind = indmin(errorvec)
    return RbfNonlinearParameters(params[:,ind],n_state, nc)
end

function plotcenters(Z)
    X = zeros(Z.n_centers,2)
    for (i,Zi) in enumerate(Z)
        X[i,:] = Zi[1:2]'
    end
    newplot(X[:,1],X[:,2],"o"); title("Centers")
end




function getΨ(Ψ, Znl, state, n_points, n_state, normalized::Bool, range = 1:1)
    Ψ = normalized ? getΨnormalized(Ψ, Znl, state, n_points, n_state) :  getΨnonnormalized(Ψ, Znl, state, n_points, n_state, range)
    if DEBUG && sum(!isfinite(Ψ)) > 0
        @show sum(!isfinite(Ψ))
    end
    return Ψ
end

function getΨnormalized(Ψ, Znl, state, n_points, n_state)
    RBF(x, Znl::VecOrMat,n_state::Integer) = exp(-(sum((x-Znl[1:n_state]).^2.*Znl[n_state+1:end])))
    rowsum = ones(n_points)
    for (j,Zin) in enumerate(Znl)
        Zi = deepcopy(Zin)
        #         for i = n_state+1:2n_state
        #             Zi[i] = Zi[i] <= 0 ? 0.01 : Zi[i] # Reset to 1 if precision became negative
        #         end
        for i = 1:n_points
            statei = squeeze(state[i,:]',2)
            a = RBF(statei, Zi, n_state)
            Ψ[i,j] = a
            rowsum[i] += a
        end
    end
    for i = 1:n_points
        if rowsum[i] <= 1e-10
            continue
        end
        @devec Ψ[i,:] ./= rowsum[i]
    end
    return Ψ
end

function getΨnonnormalized(Ψ, Znl, state, n_points, n_state, range = 1:1)
    range = (range == 1:1) ? (1:size(state,1)) : range
    RBF(x, Z,n_state) = exp(-(sum(((x-Z[1:n_state]).^2).*Z[n_state+1:end])))
    for (j,Zi) in enumerate(Znl)
        for i = range
            statei = state[i,:]'
            #                     statei = slice(state,i,:)
            Ψ[i,j] = RBF(statei, Zi, n_state)
            if DEBUG && !isfinite(Ψ[i,j])
                @show i,j,statei, Zi, n_state, Ψ[i,j]
                @show (statei-Zi[1:n_state]).^2
                @show Zi[n_state+1:end]
                error("Stopping")
            end
        end
    end

    return Ψ
end

function getLinearRegressor(V,Ψ,A,state,outputnet,na,n_state,n_centers,n_points, range = 1:1)
    outputnet ? getLinearRegressor_outputnet(V,Ψ,A,state,na,n_state,n_centers,n_points, range) : getLinearRegressor_no_outputnet(V,Ψ,A,state,na,n_state,n_centers,n_points, range)
end

function getLinearRegressor_no_outputnet(V,Ψ,A,state,na,n_state,n_centers,n_points, range = 1:1)
    range = range == 1:1 ? 1:size(state,1) : range
    ii = 1
    for i = 1:na
        for k = 1:n_centers+1
            for l = range
                V[l,ii] = Ψ[l,k]*A[l,i]
            end
            ii = ii+1
        end
    end
    if DEBUG && sum(!isfinite(V)) > 0
        @show sum(!isfinite(V))
    end
    return V
end

function getLinearRegressor_outputnet(V,Ψ,A,state,na,n_state,n_centers,n_points, range = 1:1)
    range = (range == 1:1) ? (1:size(state,1)) : range
    ii = 1
    for i = 1:na
        for k = 1:n_centers+1
            for j = 1:n_state
                for l = range
                    V[l,ii] = Ψ[l,k]*A[l,i]*state[l,j]
                end
                ii = ii+1
            end
        end
    end
    if DEBUG && sum(!isfinite(V)) > 0
        @show sum(!isfinite(V))
    end
    return V
end




function getbounds(Znl, state, n_state, n_centers)
    Lb = zeros(Znl.x)
    Ub = zeros(Znl.x)
    mas = maximum(state,1)'
    mis = minimum(state,1)'
    for i = 1:2n_state:n_centers*2n_state
        Lb[i:i+n_state-1] = mis
        Ub[i:i+n_state-1] = mas
        Lb[i+n_state:i+2n_state-1] = 0.000001
        Ub[i+n_state:i+2n_state-1] = 10*Znl.x[n_state+1:2n_state]
    end
    return Lb,Ub
end





function jacobian_outputnet(Znl, Ψ, w, V, state)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers

    n_batch = 100
    points = randperm(n_points)[1:n_batch]


    J = Array(Float64,(n_batch,length(Znl.x)))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for (l,li) = enumerate(points)
            Ψw = 1.0
            for i = 1:na
                for j = 1:n_state
                    ind = j + n_state*(k-1) + n_state*(n_centers+1)*(i-1)
                    Ψw  += V[l,ind]*w[ind]
                end
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[li,i1+p] = 2*Ψw*x_μ*γ[p]
                J[li,i1+n_state+p] = (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    return J
end

function jacobian_no_outputnet(Znl, Ψ, w,V, state)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    J = Array(Float64,(n_points,length(Znl.x)))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = 1:n_points
            Ψw = 1.0
            for i = 1:na
                ind = k + (n_centers+1)*(i-1)
                Ψw  += V[l,ind]*w[ind]
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[l,i1+p] = 2*Ψw*x_μ*γ[p]
                J[l,i1+n_state+p] = (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    return J
end


function gradient_outputnet(Znl, Ψ, V, state, A,y,w, range)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    na = size(A,2)

    #     znl = RbfNonlinearParameters(z,n_state,n_centers)
    #     psi = getΨ(Ψ, Znl, state, n_points, n_state, false, range)
    #     getLinearRegressor(V,psi,A,state,true,na,n_state,n_centers,n_points, range)
    #     w = fitlinear(V,y)

    J = zeros(length(Znl.x))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = range
            Ψw = 1.0
            for i = 1:na
                for j = 1:n_state
                    ind = j + n_state*(k-1) + n_state*(n_centers+1)*(i-1)
                    Ψw  += V[l,ind]*w[ind]
                end
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[i1+p] += 2*Ψw*x_μ*γ[p]
                J[i1+n_state+p] += (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    J /= length(range)
    return J
end

function gradient_no_outputnet(Znl, Ψ, V, state, A,y,w, range)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    na = size(A,2)
    #     psi = getΨ(Ψ, Znl, state, n_points, n_state, false, range)
    #     getLinearRegressor(V,psi,A,state,true,na,n_state,n_centers,n_points, range)
    #     w = fitlinear(V,y)
    J = zeros(length(Znl.x))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = range
            Ψw = 1.0
            for i = 1:na
                ind = k + (n_centers+1)*(i-1)
                Ψw  += V[l,ind]*w[ind]
            end
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[i1+p] += 2*Ψw*x_μ*γ[p]
                J[i1+n_state+p] += (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    J /= length(range)
    return J
end

function jacobian(Znl, Ψ, w, state)
    n_points = size(Ψ,1)
    n_state = Znl.n_state
    n_centers = Znl.n_centers
    J = Array(Float64,(n_points,length(Znl.x)))
    ii = 1
    for (k,Zi) in enumerate(Znl)
        μ = Zi[1:n_state] # slice?
        γ = Zi[n_state+1:end]
        i1 = ii-1
        for l = 1:n_points
            Ψw = Ψ[l,k]*w[k]
            for p = 1:n_state
                x_μ = state[l,p]-μ[p]
                J[l,i1+p] = 2*Ψw*x_μ*γ[p]
                J[l,i1+n_state+p] = (-Ψw)*x_μ^2
            end
        end
        ii += 2n_state
    end
    return J
end

function saturatePrecision(x,n_state)
    for i = 1:2n_state:length(x)
        range = i+n_state:i+2n_state-1
        x[range] = abs(x[range])
    end
    return x
end

function fitlinear(V,y)
    try
        DEBUG && assert(isa(V,Matrix))
        DEBUG && assert(isa(y,Vector))
        DEBUG && assert(!any(!isfinite(V)))
        DEBUG && assert(!any(!isfinite(y)))
        return V\y
    catch ex
        display(ex)
        error("Linear fitting failed")
    end
end

function Base.show(p::RbfNonlinearParameters)
    println(round(reshape(p.x,2p.n_state,p.n_centers),4))
end

function plotparams(p::RbfNonlinearParameters)
    newplot(reshape(p.x,2p.n_state,p.n_centers)',"o")
    title("Nonlinear parameters")
end