# Generate scenarios of the demands for use within the ER-SAA framework

function generateERSAAScenarios(outputs::Array{Float64,2},inputs::Array{Float64,2},covariates_obs::Array{Float64,1},regressionMethod::String,coeff_true::Array{Float64,2})

	numDependentVar::Int64 = size(outputs,2)
	numIndependentVar::Int64 = size(inputs,2)
	numSamples::Int64 = size(outputs,1)
	demand_scen_ERSAA = zeros(Float64,numSamples,numDependentVar)

	fitQuality::Float64 = 1.0

	if(size(outputs,1) != size(inputs,1))
		throw(ErrorException("Number of output samples != number of input samples"))
	end

	if(regressionMethod == "ols")

		if(numSamples < numIndependentVar)
			throw(ErrorException("Expected number of samples > number of covariates in OLS!"))
		end

		XtX_fact = lufact((inputs')*inputs)
		XtY = (inputs')*outputs

		coeff_full = zeros(Float64,numIndependentVar,numDependentVar)
		for d = 1:numDependentVar
			coeff_full[:,d] = XtX_fact\(XtY[:,d])
		end

		demand_scen_ERSAA = repeat(((covariates_obs')*coeff_full)', outer=[1,numSamples])' + outputs - inputs*coeff_full

		fitQuality = norm(coeff_full-coeff_true,2)

	elseif(regressionMethod == "lasso")

		coeff_full = getLassoCoeff(outputs,inputs)

		demand_scen_ERSAA = repeat(((covariates_obs')*coeff_full)', outer=[1,numSamples])' + outputs - inputs*coeff_full

		fitQuality = norm(coeff_full-coeff_true,2)

	end

	return demand_scen_ERSAA, fitQuality
end


Actions = Dict('N' => [0, 1], 'S' => [0, -1], 'E' => [1, 0], 'W' => [-1, 0])

struct Instr
    act::Char
    val::Int
end

puzzleinput = readlines("inputs/day12.txt")
puzzleinput = map(s -> Instr(s[1], parse(Int, s[2:end])), puzzleinput)

# First Half

struct Ship
    pos::Vector{Int} # Ship position on the plane (x, y)
    dir::Vector{Int} # Ship direction as unit vector (eg. ovest = (-1,0))
end
# The ship status

ship = Ship([0, 0], [1, 0]) # The ship starts by facing east.

function handle1(s::Ship, i::Instr)
    # From a ship status s, handles an instruction i returning the new ship status

    act, val = i.act, i.val
    if act == 'L' || act == 'R'
        val = (act == 'R' ? -1 : 1) * val
        #  Right means clockwise rotation = negatives angles in trigs functions

        θ = deg2rad(val)
        R = trunc.(Int, [cos(θ) -sin(θ); sin(θ) cos(θ)])
        return Ship(s.pos, R * s.dir)
        # To perform rotation, multiply by rotation matrix

    end
    if act == 'F'
        v = val * s.dir
    else
        v = val * Actions[act]
    end
    Ship(s.pos + v, s.dir)
end

wherelead = reduce(handle1, puzzleinput, init=ship)

sum(map(abs, wherelead.pos))

# Second Half

function handle2(sw::Tuple{Ship,Array{Int,1}}, i::Instr)
    # From a ship status s and a waypoint w,
    # handles an instruction i returning the new ship status and waypoint

    s, w = sw[1], sw[2]
    act, val = i.act, i.val
    if act == 'L' || act == 'R'
        val = (act == 'R' ? -1 : 1) * val
        θ = deg2rad(val)
        R = trunc.(Int, [cos(θ) -sin(θ); sin(θ) cos(θ)])
        return (s, R * w)
    end
    if act == 'F'
        v = val * w
        s = Ship(s.pos + v, s.dir)
    else
        v = val * Actions[act]
        w += v
    end
    (s, w)
end

waypoint = ship.pos + [10, 1]
# The waypoint starts 10 units east and 1 unit north relative to the ship.

wherelead = reduce(handle2, puzzleinput, init=(ship, waypoint))

sum(map(abs, wherelead[1].pos))


using SparseArrays
using FastGaussQuadrature

include("SplineModel.jl")
include("MeshUtils.jl")

struct Boundary1D
    type::String #can be "Dirichlet, Neumann or Robin"
    x_val::Float64 #boundary point in the physical space
    u_val::Float64 #boundary point in the parameter space
    α::Number #parameter value for Robin boundary condition (αu+u')
    op_val::Number #boundary operator value
end

function Boundary1D(type::String, x_val::Float64, u_val::Float64, op_val::Number)
    return Boundary1D(type, x_val, u_val, 0., op_val)
end

struct Problem1D
    rhs_f::Function
    a0::Function
    a1::Function
    boundary_cond::Array{Boundary1D}
    domain::NURBS
end

struct GaussQuad
    nodes::Array{Float64,1}
    weights::Array{Float64,1}
end

"""
Generates the Gauss-Legendre points of order n
"""
function genGaussLegendre(n)
    nodes, weights = gausslegendre(n)
    gauss_rule = GaussQuad(nodes, weights)
    return gauss_rule
end

"""
Aseembles the stiffness matrix K_ij = ∫ a0(x)ϕ_i'(x)ϕ_j'(x) dΩ
"""
function assemble_stiff(mesh::Mesh, a0::Function, gauss_rule)
    B, dB = bernsteinBasis(gauss_rule.nodes, degP)
    domainLength = 0
    stiff = spzeros(mesh.numBasis, mesh.numBasis)
    for iElem = 1:mesh.numElem
        uMin = mesh.elemVertex[iElem, 1]
        uMax = mesh.elemVertex[iElem, 2]
        Jac_ref_par = (uMax-uMin)/2

        #compute the (B-)spline basis functions and derivatives with Bezier extraction
        N_mat = B * mesh.C[iElem]'
        dN_mat = dB * mesh.C[iElem]'/Jac_ref_par

        #compute the rational spline basis
        curNodes = mesh.elemNode[iElem]
        numNodes = length(curNodes)
        cpts = mesh.controlPoints[1, curNodes]
        wgts = mesh.weights[curNodes]
        localStiff = zeros(numNodes, numNodes)
        for iGauss = 1:length(gauss_rule.nodes)
            #compute the rational basis
            RR = N_mat[iGauss,:].* wgts
            dR = dN_mat[iGauss,:].* wgts
            w_sum = sum(RR)
            dw_xi = sum(dR)
            dR = dR/w_sum - RR*dw_xi/w_sum^2

            #compute the derivatives w.r.t to the physical space
            dxdxi = dR' * cpts
            dR = dxdxi\dR
            Jac_par_phys = det(dxdxi)

            RR /= w_sum
            phys_pt = RR'*cpts
            localStiff += Jac_par_phys * Jac_ref_par * a0(phys_pt) * (dR*dR') * gauss_rule.weights[iGauss]
            domainLength += Jac_par_phys * Jac_ref_par * gauss_rule.weights[iGauss]

        end
        stiff[curNodes, curNodes] += localStiff

    end
    @show domainLength
    return stiff
end

"""
Aseembles the mass matrix M_ij = ∫ a1(x)ϕ_i(x)ϕ_j(x) dΩ
"""
function assemble_mass(mesh::Mesh, a1::Function, gauss_rule)
    B, dB = bernsteinBasis(gauss_rule.nodes, degP)
    domainLength = 0
    mass = spzeros(Complex{Float64}, mesh.numBasis, mesh.numBasis)
    for iElem = 1:mesh.numElem
        uMin = mesh.elemVertex[iElem, 1]
        uMax = mesh.elemVertex[iElem, 2]
        Jac_ref_par = (uMax-uMin)/2

        #compute the (B-)spline basis functions and derivatives with Bezier extraction
        N_mat = B * mesh.C[iElem]'
        dN_mat = dB * mesh.C[iElem]'/Jac_ref_par

        #compute the rational spline basis
        curNodes = mesh.elemNode[iElem]
        numNodes = length(curNodes)
        cpts = mesh.controlPoints[1, curNodes]
        wgts = mesh.weights[curNodes]
        localMass = zeros(numNodes, numNodes)
        for iGauss = 1:length(gauss_rule.nodes)
            #compute the rational basis
            RR = N_mat[iGauss,:].* wgts
            dR = dN_mat[iGauss,:].* wgts
            w_sum = sum(RR)
            dw_xi = sum(dR)
            dR = dR/w_sum - RR*dw_xi/w_sum^2

            #compute the Jacobian of the transformation from parameter to physical space
            dxdxi = dR' * cpts
            Jac_par_phys = det(dxdxi)

            RR /= w_sum
            phys_pt = RR'*cpts
            localMass += a1(phys_pt) * (RR*RR') * Jac_par_phys * Jac_ref_par * gauss_rule.weights[iGauss]
            domainLength += Jac_par_phys * Jac_ref_par * gauss_rule.weights[iGauss]

        end
        #@show localMass
        #readline(stdin)
        mass[curNodes, curNodes] += localMass
    end
    @show domainLength
    return mass
end

"""
Assembles the RHS vector corresponding to the body force
RHS[i]=∫_Ω ϕ_i(x)*f(x) dΩ
"""
function assemble_rhs(mesh::Mesh, f::Function, gauss_rule)
    B, dB = bernsteinBasis(gauss_rule.nodes, degP)
    domainLength = 0
    rhs = zeros(Complex{Float64}, mesh.numBasis)
    for iElem = 1:mesh.numElem
        uMin = mesh.elemVertex[iElem, 1]
        uMax = mesh.elemVertex[iElem, 2]
        Jac_ref_par = (uMax-uMin)/2
        #@show Jac_ref_par
        #compute the (B-)spline basis functions and derivatives with Bezier extraction
        N_mat = B * mesh.C[iElem]'
        dN_mat = dB * mesh.C[iElem]'/Jac_ref_par

        #compute the rational spline basis
        curNodes = mesh.elemNode[iElem]
        numNodes = length(curNodes)
        cpts = mesh.controlPoints[1, curNodes]
        wgts = mesh.weights[curNodes]
        localRhs = zeros(numNodes)
        for iGauss = 1:length(gauss_rule.nodes)
            #compute the rational basis
            RR = N_mat[iGauss,:].* wgts
            dR = dN_mat[iGauss,:].* wgts
            w_sum = sum(RR)
            dw_xi = sum(dR)
            dR = dR/w_sum - RR*dw_xi/w_sum^2

            #compute the derivatives w.r.t to the physical space
            dxdxi = dR' * cpts
            Jac_par_phys = det(dxdxi)
            RR /= w_sum

            phys_pt = RR'*cpts
            localRhs += Jac_par_phys * Jac_ref_par * f(phys_pt) * RR * gauss_rule.weights[iGauss]
            domainLength += Jac_par_phys * Jac_ref_par * gauss_rule.weights[iGauss]
        end
        rhs[curNodes] += localRhs
    end
    @show domainLength
    return rhs
end

function uniquetol(arr, tol=1e-10)
    sort!(arr)
    delete_index = Array{Int64,1}(undef, 0)
    for i=1:length(arr)-1
        if abs(arr[i+1]-arr[i])<tol
            push!(delete_index, i+1)
        end
    end
    deleteat!(arr, delete_index)
end

"""
Helper function to compute the setdifference within a tolerance
"""
function setdifftol(arrA, arrB, tol=1e-10)
    delete_index = Array{Int64,1}(undef, 0)
    for i=1:length(arrA)
        for j=1:length(arrB)
            if abs(arrA[i]-arrB[j])<tol
                push!(delete_index, i)
                break
            end
        end
    end
    deleteat!(arrA, delete_index)
end

"""
Apply the boundary conditions to a linear system for a 1D
nurbs geometry
"""
function applyBCnurbs(bound_cond, stiff, mass, rhs, nrb)
    #collect the dofs and values corresponding to the boundary
    lhs = stiff+mass
    bcdof = Array{Int64,1}(undef, 0)
    bcval = Array{Float64,1}(undef, 0)
    for i=1:length(bound_cond)

        if bound_cond[i].type=="Dirichlet"
            bcdof = vcat(bcdof, findall(nrb.coefs[1,:].==bound_cond[i].x_val))
            bcval = vcat(bcval, bound_cond[i].op_val)
        elseif bound_cond[i].type=="Neumann"
            bcdof_neu = findall(nrb.coefs[1,:].==bound_cond[i].x_val)
            rhs[bcdof_neu] .+= bound_cond[i].op_val
        elseif bound_cond[i].type=="Robin"
            #Assume that the basis functions have value 1 on the Robin boundary
            bcdof_rob = findall(nrb.coefs[1,:].==bound_cond[i].x_val)
            lhs[bcdof_rob, bcdof_rob] .+= bound_cond[i].α
            rhs[bcdof_rob] .+= bound_cond[i].op_val
        else
            error("Boundary condition type not implemented")
        end
    end
    rhs = rhs - lhs[:,bcdof]*bcval
    rhs[bcdof] = bcval
    lhs[bcdof, :] .= 0.
    lhs[:, bcdof] .= 0.
    lhs[bcdof, bcdof] = sparse(I, length(bcdof), length(bcdof))
    return lhs, rhs
end


#
# Count the number of hydrogen bonds between two selections
#

# If the coordinates of a PDB file read into the atoms vector is given
# Bonds are needed, because we need to know to which atoms the hydrogens
# are bound to
#

dotp(x,y) = x[1]*y[1]+x[2]*y[2]+x[3]*y[3]
vnorm(x) = sqrt(x[1]^2 + x[2]^2 + x[3]^2)
cosang(x,y) = dotp(x,y) / (vnorm(x)*vnorm(y))

function nhbonds(atoms,bonds,sel1,sel2; d = 3., angle = 20. )

  dhbond2 = d^2
  coshbond = cos(pi*angle/180.)

  n1 = length(sel1)
  n2 = length(sel2)

  nhbonds = 0
  for isel in 1:n1
    i = sel1[isel]
    for jsel in 1:n2
      j = sel2[jsel]
      # If one of the atoms is not electronegative, skip
      if ! ( atoms[i].element in [ "N", "O", "F" ] && atoms[j].element in [ "N", "O", "F" ] )
        continue
      end
      # If the distance is not compatible, skip
      d2 = (atoms[i].coor[1] - atoms[j].coor[1])^2 +
           (atoms[i].coor[2] - atoms[j].coor[2])^2 +
           (atoms[i].coor[3] - atoms[j].coor[3])^2
      if d2 > dhbond2
        continue
      end
      # Check if a hydrogen is bound to atom i
      foundhbond = false
      ibonds = filter( bond -> ( bond.i == i || bond.j == i ), bonds )
      for bond in ibonds
        if atoms[bond.i].element == "H"
          ib = bond.i
        elseif atoms[bond.j].element == "H"
          ib = bond.j
        else
          continue
        end
        v1H = [ atoms[ib].coor[1] - atoms[i].coor[1] ,
                atoms[ib].coor[2] - atoms[i].coor[2] ,
                atoms[ib].coor[3] - atoms[i].coor[3] ]
        vH2 = [ atoms[j].coor[1] - atoms[ib].coor[1] ,
                atoms[j].coor[2] - atoms[ib].coor[2] ,
                atoms[j].coor[3] - atoms[ib].coor[3] ]
        if cosang(v1H,vH2) > coshbond
          nhbonds = nhbonds + 1
          foundhbond = true
          break
        end
      end
      # If the hbond is already found between these to heavy atoms, skip next
      if foundhbond
        continue
      end
      # Check if a hydrogen is bound to atom j
      jbonds = filter( bond -> ( bond.i == j || bond.j == j ), bonds )
      for bond in jbonds
        if atoms[bond.i].element == "H"
          ib = bond.i
        elseif atoms[bond.j].element == "H"
          ib = bond.j
        else
          continue
        end
        v1H = [ atoms[ib].coor[1] - atoms[j].coor[1] ,
                atoms[ib].coor[2] - atoms[j].coor[2] ,
                atoms[ib].coor[3] - atoms[j].coor[3] ]
        vH2 = [ atoms[i].coor[1] - atoms[ib].coor[1] ,
                atoms[i].coor[2] - atoms[ib].coor[2] ,
                atoms[i].coor[3] - atoms[ib].coor[3] ]
        if cosang(v1H,vH2) > coshbond
          nhbonds = nhbonds + 1
          break
        end
      end
    end
  end

  return nhbonds
end


function preconditioner(d, q)
    P_preallocated = Diagonal(zeros(d*(q+1)))

    @fastmath @inbounds function P(h)
        val = h^(-q-1/2)
        @simd for j in 0:q
            @simd for i in 1:d
                # P_preallocated[j*d + i,j*d + i] = h^(j-q-1/2)
                P_preallocated[j*d + i,j*d + i] = val
            end
            val *= h
        end
        return P_preallocated
    end

    return P
end


using FractionalDiffEq

function ϕ(x)
    if x == 0
        return 19.00001
    else
        return 19.0
    end
end

f(t, y, ϕ) = 3.5*y*(1-ϕ/19)

h = 0.05
α = 0.97
τ = 0.8
T = 56
fddeprob = FDDEProblem(f, ϕ, α, τ, T)
V, y = solve(fddeprob, h, DelayPECE())

using Plots
plot(y, V, xlabel="y(t)", ylabel="y(t-τ)")


@testset "replace!" begin
    y = [1, 2, 2, 2, 3]
    z1 = 1:5
    z2 = 0:3
    r1 = -2:2
    r2 = 2:5
    @test_throws DimensionMismatch _replace!(y, z1, r2)
    @test_throws DimensionMismatch _replace!(y, z2, r1)
    @test _replace!(deepcopy(y), z1, r1) == Base.replace!(deepcopy(y), (z1 .=> r1)...)
    @test _replace!(deepcopy(y), z2, r2) == Base.replace!(deepcopy(y), (z2 .=> r2)...)
end

@testset "softmax" begin
    X = rand(100,5)
    max_ = maximum(X, dims=2)
    exp_ = exp.(X .- max_)
    @test isapprox(MLJMultivariateStatsInterface.softmax(X), exp_ ./ sum(exp_, dims=2))
    X_ = MLJMultivariateStatsInterface.softmax!(X)
    @test X_ === X
    @test isapprox(X_, exp_ ./ sum(exp_, dims=2))
end


module baffled_piston_examples
using FinEtools
using FinEtoolsAcoustics
using LinearAlgebra

function baffled_piston_H8_ABC_example_algo()
    rho = 1.21*phun("kg/m^3");# mass density
    c  = 343.0*phun("m/s");# sound speed
    bulk =  c^2*rho;
    omega =  7500*phun("rev/s");      # frequency of the piston
    a_piston =  -1.0*phun("mm/s")     # amplitude of the piston acceleration
    R = 50.0*phun("mm");# radius of the piston
    Ro = 150.0*phun("mm"); # radius of the enclosure
    nref = 4;#number of refinements of the sphere around the piston
    nlayers = 35;                     # number of layers of elements surrounding the piston
    tolerance = R/(2^nref)/100

    println("""

    Baffled piston in a half-sphere domain with ABC.

    Hexahedral mesh. Algorithm version.
    """)

    t0  =  time()

    # Hexahedral mesh
    fens,fes  =  H8sphere(R,nref);
    bfes  =  meshboundary(fes)
    File  =   "baffledabc_boundary.vtk"
    vtkexportmesh(File, connasarray(bfes), fens.xyz, FinEtools.MeshExportModule.Q4)
    @async run(`"paraview.exe" $File`)

    l = selectelem(fens,bfes,facing = true,direction = [1.0 1.0  1.0], dotmin= 0.001)
    ex(xyz, layer) = (R+layer/nlayers*(Ro-R))*xyz/norm(xyz)
    fens1,fes1  =  H8extrudeQ4(fens, subset(bfes,l), nlayers, ex);
    fens,newfes1,fes2 =  mergemeshes(fens1, fes1, fens, fes, tolerance)
    fes = cat(newfes1,fes2)

    # Piston surface mesh
    bfes  =  meshboundary(fes)
    l1 = selectelem(fens, bfes, facing = true, direction = [-1.0 0.0 0.0])
    l2 = selectelem(fens, bfes, distance = R, from = [0.0 0.0 0.0], inflate = tolerance)
    piston_fes = subset(bfes,intersect(l1,l2));

    # Outer spherical boundary
    louter = selectelem(fens, bfes, facing = true, direction = [1.0 1.0  1.0], dotmin= 0.001)
    outer_fes = subset(bfes,louter);

    println("Pre-processing time elapsed  =  ",time() - t0,"s")

    t1  =  time()

    material = MatAcoustFluid(bulk, rho)
    # Region of the fluid
    region1 =  FDataDict("femm"=>FEMMAcoust(IntegDomain(fes, GaussRule(3, 2)), material))

    # Surface for the ABC
    abc1  =  FDataDict("femm"=>FEMMAcoustSurf(IntegDomain(outer_fes, GaussRule(2, 2)),
    material))

    # Surface of the piston
    flux1  =  FDataDict("femm"=>FEMMAcoustSurf(IntegDomain(piston_fes, GaussRule(2, 2)),
    material),  "normal_flux"=> -rho*a_piston+0.0im);

    # Make model data
    modeldata =  FDataDict("fens"=>  fens,
    "omega"=>omega,
    "regions"=>[region1],
    "flux_bcs"=>[flux1], "ABCs"=>[abc1])

    # Call the solver
    modeldata = FinEtoolsAcoustics.AlgoAcoustModule.steadystate(modeldata)

    println("Computing time elapsed  =  ",time() - t1,"s")
    println("Total time elapsed  =  ",time() - t0,"s")

    geom = modeldata["geom"]
    P = modeldata["P"]

    File  =   "baffledabc.vtk"
    vtkexportmesh(File, connasarray(fes), geom.values, FinEtools.MeshExportModule.H8;
    scalars = [("absP", abs.(P.values))])
    @async run(`"paraview.exe" $File`)
end # baffled_piston_H8_ABC_example_algo

function allrun()
    println("#####################################################")
    println("# baffled_piston_H8_ABC_example_algo ")
    baffled_piston_H8_ABC_example_algo()
    return true
end # function allrun

end # module baffled_piston_examples


#=--------------------------------------------------------------------

ransac - Functions for robust estimation using RANSAC

Copyright (c) 2016 Peter Kovesi
pk@peterkovesi.com

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

The Software is provided "as is", without warranty of any kind.

PK February 2016

---------------------------------------------------------------------=#

export ransac
export ransacfithomography, ransacfitfundmatrix, ransacfitaffinefundmatrix
export ransacfitplane, ransacfitline
export iscolinear, fitline2d, fitline3d, fitplane

#---------------------------------------------------------------------
"""
ransac - Robustly fits a model to data with the RANSAC algorithm
```
Usage:

(M, inliers) = ransac(x, fittingfn, distfn, degenfn s, t, feedback,
                      maxDataTrials, maxTrials)

Arguments:
    x         - Data sets to which we are seeking to fit a model M
                It is assumed that x is of size [d x Npts]
                where d is the dimensionality of the data and Npts is
                the number of data points.

    fittingfn - Reference to a function that fits a model to s
                data from x.  It is assumed that the function is of the
                form:
                   M = fittingfn(x)
                Note it is possible that the fitting function can return
                multiple models (for example up to 3 fundamental matrices
                can be fitted to 7 pairs of matched points).  In this
                case it is assumed that the fitting function returns an
                array of models.
                If this function cannot fit a model it should return M as
                an empty matrix.

    distfn    - Reference to a function that evaluates the
                distances from the model to data x.
                It is assumed that the function is of the form:
                   (inliers, M) = distfn(M, x, t)
                This function must evaluate the distances between points
                and the model returning the indices of elements in x that
                are inliers, that is, the points that are within distance
                't' of the model.  Additionally, if M is an array of
                possible models distfn() will return the model that has the
                most inliers.  If there is only one model this function
                must still copy the model to the output.  After this call M
                will be a single object representing only one model.

    degenfn   - Reference to a function that determines whether a
                set of datapoints will produce a degenerate model.
                This is used to discard random samples that do not
                result in useful models.
                It is assumed that degenfn is a boolean function of
                the form:
                   r = degenfn(x)
                It may be that you cannot devise a test for degeneracy in
                which case you should write a dummy function that always
                returns a value of true and rely on fittingfn() to return
                an empty model should the data set be degenerate.

    s         - The minimum number of samples from x required by
                fittingfn to fit a model.

    t         - The distance threshold between a data point and the model
                used to decide whether the point is an inlier or not.

    feedback  - An optional boolean flag. If set to true the trial count and the
                estimated total number of trials required is printed out at
                each step.  Defaults to false.

maxDataTrials - Maximum number of attempts to select a non-degenerate
                data set. This parameter is optional and defaults to 100.

    maxTrials - Maximum number of iterations. This parameter is optional and
                defaults to 1000.

    p         - Desired probability of choosing at least one sample
                free from outliers, defaults to 0.99

Returns:
    M         - The model having the greatest number of inliers.
    inliers   - An array of indices of the elements of x that were
                the inliers for the best model.
```
For an example of the use of this function see ransacfithomography() or
ransacfitplane()
"""
#=
References:
   M.A. Fishler and  R.C. Boles. "Random sample concensus: A paradigm
   for model fitting with applications to image analysis and automated
   cartography". Comm. Assoc. Comp, Mach., Vol 24, No 6, pp 381-395, 1981

   Richard Hartley and Andrew Zisserman. "Multiple View Geometry in
   Computer Vision". pp 101-113. Cambridge University Press, 2001

May      2003 - Original version in MATLAB
February 2004 - Tidied up.
August   2005 - Specification of distfn changed to allow model fitter to
                return multiple models from which the best must be selected
Sept     2006 - Random selection of data points changed to ensure duplicate
                points are not selected.
December 2008 - Octave compatibility mods
June     2009 - Argument 'MaxTrials' corrected to 'maxTrials'!
February 2016 - Ported to Julia
=#

function ransac(x, fittingfn, distfn, degenfn, s, t, feedback = false,
                maxDataTrials = 1000, maxTrials = 1000, p = 0.99)

    (rows, npts) = size(x)

    if npts < s
        error("Not enough data points to form a model")
    end

    bestM = NaN      # Sentinel value allowing detection of solution failure.
    trialcount = 0
    bestscore =  0
    bestinliers = []  # Ensure global to function
    bestM = []
    N = 1            # Dummy initialisation for number of trials.

    while N > trialcount

        # Select at random s datapoints to form a trial model, M.
        # In selecting these points we have to check that they are not in
        # a degenerate configuration.
        degenerate = true
        count = 1
        while degenerate
            # Generate s random indicies in the range 1..npts
            ind = randperm(npts)[1:s]

            # Test that these points are not a degenerate configuration.
            degenerate = degenfn(x[:,ind])

            if !degenerate
                # Fit model to this random selection of data points.
                # Note that M may represent a set of models that fit the data in
                # this case M will be a cell array of models
                M = fittingfn(x[:,ind])

                # Depending on your problem it might be that the only way you
                # can determine whether a data set is degenerate or not is to
                # try to fit a model and see if it succeeds.  If it fails we
                # reset degenerate to true.
                if isempty(M)
                    degenerate = true
                end
            end

            # Safeguard against being stuck in this loop forever
            count += 1
            if count > maxDataTrials
                warn("Unable to select a nondegenerate data set")
                break
            end
        end

        # Once we are out here we should have some kind of model...
        # Evaluate distances between points and model returning the indices
        # of elements in x that are inliers.  Additionally, if M is a cell
        # array of possible models 'distfn' will return the model that has
        # the most inliers.  After this call M will be a non-cell object
        # representing only one model.
        (inliers, M) = distfn(M, x, t)

        # Find the number of inliers to this model.
        ninliers = length(inliers)

        if ninliers > bestscore    # Largest set of inliers so far...
            bestscore = ninliers   # Record data for this model
            bestinliers = copy(inliers)
            bestM = copy(M)

            # Update estimate of N, the number of trials to ensure we pick,
            # with probability p, a data set with no outliers.
            fracinliers =  ninliers/npts
            pNoOutliers = 1 -  fracinliers^s
            pNoOutliers = max(eps(), pNoOutliers)  # Avoid division by -Inf
            pNoOutliers = min(1-eps(), pNoOutliers)# Avoid division by 0.
            N = log(1-p)/log(pNoOutliers)
        end

        trialcount += 1
        if feedback
            @printf("trial %d out of %d         \r",trialcount, ceil(N))
        end

        # Safeguard against being stuck in this loop forever
        if trialcount > maxTrials
            @printf("RANSAC reached the maximum number of %d trials", maxTrials)
            break
        end
    end

    if feedback; @printf("\n"); end

    if !any(isnan(bestM))   # We got a solution
        M = bestM
        inliers = bestinliers
    else
        M = []
        inliers = []
        error("RANSAC was unable to find a useful solution")
    end

    return M, inliers
end

#-----------------------------------------------------------------------
"""
ransacfithomography - Fits 2D homography using RANSAC
```
Usage:   (H, inliers) = ransacfithomography(x1, x2, t)

Arguments:
         x1  - 2xN or 3xN set of homogeneous points.  If the data is
               2xN it is assumed the homogeneous scale factor is 1.
         x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.
         t   - The distance threshold between data point and the model
               used to decide whether a point is an inlier or not.
               Note that point coordinates are normalised to that their
               mean distance from the origin is sqrt(2).  The value of
               t should be set relative to this, say in the range
               0.001 - 0.01
```
Note that it is assumed that the matching of x1 and x2 are putative and it
is expected that a percentage of matches will be wrong.
```
Returns:
         H       - The 3x3 homography such that x2 = H*x1.
         inliers - An array of indices of the elements of x1, x2 that were
                   the inliers for the best model.
```
See Also: ransac(), homography2d(), homography1d()
"""

#=
February 2004 - original version
July     2004 - error in denormalising corrected (thanks to Andrew Stein)
August   2005 - homogdist2d modified to fit new ransac specification.
=#

function ransacfithomography(x1i, x2i, t::Real)

    # Define some subfunctions

    #----------------------------------------------------------------------
    # Function to evaluate the symmetric transfer error of a homography with
    # respect to a set of matched points as needed by RANSAC.

    function homogdist2d(H, x, t)

        x1 = x[1:3,:]   # Extract x1 and x2 from x
        x2 = x[4:6,:]

        # Calculate, in both directions, the transfered points
        Hx1    = H*x1
        invHx2 = H\x2

        # Normalise so that the homogeneous scale parameter for all coordinates
        # is 1.
        x1     = hnormalise(x1)
        x2     = hnormalise(x2)
        Hx1    = hnormalise(Hx1)
        invHx2 = hnormalise(invHx2)

        d2 = sum((x1-invHx2).^2, 1)  + sum((x2-Hx1).^2, 1)
        inliers = find(abs(d2) .< t)
        return inliers, H
    end

    #----------------------------------------------------------------------
    # Function to determine if a set of 4 pairs of matched  points give rise
    # to a degeneracy in the calculation of a homography as needed by RANSAC.
    # This involves testing whether any 3 of the 4 points in each set is
    # colinear.

    function isdegenerate(x)

        (rows, npts) = size(x)
        assert(npts == 4)

        x1 = x[1:3,:]    # Extract x1 and x2 from x
        x2 = x[4:6,:]

        r =
        iscolinear(x1[:,1],x1[:,2],x1[:,3]) ||
        iscolinear(x1[:,1],x1[:,2],x1[:,4]) ||
        iscolinear(x1[:,1],x1[:,3],x1[:,4]) ||
        iscolinear(x1[:,2],x1[:,3],x1[:,4]) ||
        iscolinear(x2[:,1],x2[:,2],x2[:,3]) ||
        iscolinear(x2[:,1],x2[:,2],x2[:,4]) ||
        iscolinear(x2[:,1],x2[:,3],x2[:,4]) ||
        iscolinear(x2[:,2],x2[:,3],x2[:,4])

        return r
    end
    #----------------------------------------------------------

    # The main function

    if size(x1i) != size(x2i)
        error("Data sets x1 and x2 must have the same dimensions")
    end

    (rows,npts) = size(x1i)
    if !(rows == 2 || rows == 3)
        error("x1 and x2 must have 2 or 3 rows")
    end

    if npts < 4
        error("Must have at least 4 points to fit homography")
    end

    if rows == 2    # Pad data with homogeneous scale factor of 1
        x1 = [x1i; ones(1,npts)]
        x2 = [x2i; ones(1,npts)]
    else
        x1 = copy(x1i)
        x2 = copy(x2i)
    end

    # Normalise each set of points so that the origin is at centroid and
    # mean distance from origin is sqrt(2).  normalise2dpts also ensures the
    # scale parameter is 1.  Note that 'homography2d' will also call
    # 'normalise2dpts' but the code in 'ransac' that calls the distance
    # function will not - so it is best that we normalise beforehand.
    (x1, T1) = normalise2dpts(x1)
    (x2, T2) = normalise2dpts(x2)

    s = 4  # Minimum No of points needed to fit a homography.

    fittingfn = homography2d
    distfn    = homogdist2d
    degenfn   = isdegenerate
    # x1 and x2 are 'stacked' to create a 6xN array for ransac
    (H, inliers) = ransac([x1; x2], fittingfn, distfn, degenfn, s, t)

    # Now do a final least squares fit on the data points considered to
    # be inliers.
    H = homography2d(x1[:,inliers], x2[:,inliers])

    # Denormalise
    H = T2\H*T1

    return H, inliers

end # of ransacfithomography

#-----------------------------------------------------------------------
"""
ransacfitfundmatrix - Fits fundamental matrix using RANSAC
```
Usage:   (F, inliers) = ransacfitfundmatrix(x1, x2, t)

Arguments:
         x1  - 2xN or 3xN set of homogeneous points.  If the data is
               2xN it is assumed the homogeneous scale factor is 1.
         x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.
         t   - The distance threshold between data point and the model
               used to decide whether a point is an inlier or not.
               Note that point coordinates are normalised to that their
               mean distance from the origin is sqrt(2).  The value of
               t should be set relative to this, say in the range
               0.001 - 0.01
```
Note that it is assumed that the matching of x1 and x2 are putative and it
is expected that a percentage of matches will be wrong.
```
Returns:
         F       - The 3x3 fundamental matrix such that x2'Fx1 = 0.
         inliers - An array of indices of the elements of x1, x2 that were
                   the inliers for the best model.
```
See also: ransac(), fundmatrix()
"""

# February 2004  Original version
# August   2005  Distance error function changed to match changes in RANSAC

function ransacfitfundmatrix(x1i, x2i, t::Real, feedback::Bool = false)

    # Define some subfunctions

    #--------------------------------------------------------------------------
    # Function to evaluate the first order approximation of the geometric error
    # (Sampson distance) of the fit of a fundamental matrix with respect to a
    # set of matched points as needed by RANSAC.  See: Hartley and Zisserman,
    # 'Multiple View Geometry in Computer Vision', page 270.
    #
    # Note that this code allows for F being an array of fundamental matrices of
    # which we have to pick the best one. (A 7 point solution can return up to 3
    # solutions)

    function funddist(F, x, t)

        npts = size(x, 2)
        x1 = x[1:3,:]    # Extract x1 and x2 from x
        x2 = x[4:6,:]

        if isa(F, Array{Any})  # We have several solutions each of which must be tested
	    nF = length(F)   # Number of solutions to test
	    bestF = F[1]     # Initial allocation of best solution
	    ninliers = 0     # Number of inliers

	    for k = 1:nF
	        x2tFx1 = zeros(npts)
	        for n = 1:npts
		    x2tFx1[n] = (x2[:,n:n]'*F[k]*x1[:,n:n])[1]
	        end

	        Fx1 = F[k]*x1
	        Ftx2 = F[k]'*x2

	        # Evaluate distances
	        d =  x2tFx1.^2 ./
		vec(Fx1[1,:].^2 + Fx1[2,:].^2 + Ftx2[1,:].^2 + Ftx2[2,:].^2)

	        inliers = find(abs(d) .< t)     # Indices of inlying points

	        if length(inliers) > ninliers   # Record best solution
		    ninliers = length(inliers)
		    bestF = copy(F[k])
		    bestInliers = copy(inliers)
	        end
	    end

        else     # We just have one solution
	    x2tFx1 = zeros(npts)
	    for n = 1:npts
	        x2tFx1[n] = (x2[:,n:n]'*F*x1[:,n:n])[1] # Ugly thanks to v0.5
	    end

	    Fx1 = F*x1
	    Ftx2 = F'*x2

	    # Evaluate distances (The call to vec() needed for 0.4)
	    d =  x2tFx1.^2 ./
	    vec(Fx1[1,:].^2 + Fx1[2,:].^2 + Ftx2[1,:].^2 + Ftx2[2,:].^2)

	    bestInliers = find(abs(d) .< t)     # Indices of inlying points
	    bestF = F                          # Copy F directly to bestF
        end

        return bestInliers, bestF
    end

    #----------------------------------------------------------------------
    # (Degenerate!) function to determine if a set of matched points will result
    # in a degeneracy in the calculation of a fundamental matrix as needed by
    # RANSAC.  This function assumes this cannot happen...

    function isdegenerate(x)
        return false
    end

    #----------------------------------------------------------------------
    # The main function

    if size(x1i) != size(x2i)
        error("Data sets x1 and x2 must have the same dimension")
    end

    (rows, npts) = size(x1i)
    if !(rows==2 || rows==3)
        error("x1 and x2 must have 2 or 3 rows")
    end

    if rows == 2    # Pad data with homogeneous scale factor of 1
        x1 = makehomogeneous(x1i)
        x2 = makehomogeneous(x2i)
    else
        x1 = copy(x1i)
        x2 = copy(x2i)
    end

    # Normalise each set of points so that the origin is at centroid and
    # mean distance from origin is sqrt(2).  normalise2dpts also ensures the
    # scale parameter is 1.  Note that fundmatrix() will also call
    # normalise2dpts() but the code in ransac() that calls the distance
    # function will not - so it is best that we normalise beforehand.
    (x1, T1) = normalise2dpts(x1)
    (x2, T2) = normalise2dpts(x2)

    s = 8  # Number of points needed to fit a fundamental matrix. Note that
           # only 7 are needed but the function fundmatrix() only
           # implements the 8-point solution.

    fittingfn = fundmatrix
    distfn    = funddist
    degenfn   = isdegenerate
    # x1 and x2 are 'stacked' to create a 6xN array for ransac
    (F, inliers) = ransac([x1; x2], fittingfn, distfn, degenfn, s, t, feedback)

    # Now do a final least squares fit on the data points considered to
    # be inliers.
    F = fundmatrix(x1[:,inliers], x2[:,inliers])

    # Denormalise
    F = T2'*F*T1

    return F, inliers

end # of ransacfitfundmatrix()

#-----------------------------------------------------------------------
"""
ransacfitaffinefundmatrix - Fits affine fundamental matrix using RANSAC
```
Usage:   (F, inliers) = ransacfitaffinefundmatrix(x1, x2, t)

Arguments:
         x1  - 2xN or 3xN set of homogeneous points.  If the data is
               2xN it is assumed the homogeneous scale factor is 1.
         x2  - 2xN or 3xN set of homogeneous points such that x1<->x2.
         t   - The distance threshold between data point and the model
               used to decide whether a point is an inlier or not.
               Note that point coordinates are normalised to that their
               mean distance from the origin is sqrt(2).  The value of
               t should be set relative to this, say in the range
               0.001 - 0.01
```
Note that it is assumed that the matching of x1 and x2 are putative and it
is expected that a percentage of matches will be wrong.
```
Returns:
         F       - The 3x3 fundamental matrix such that x2'Fx1 = 0.
         inliers - An array of indices of the elements of x1, x2 that were
                   the inliers for the best model.
```
See also: ransac(), fundmatrix(), affinefundmatrix()
"""

# February 2004  Original version
# August   2005  Distance error function changed to match changes in RANSAC

function ransacfitaffinefundmatrix(x1i, x2i, t::Real, feedback::Bool = false)

    # Define some subfunctions
    #--------------------------------------------------------------------------
    # Function to evaluate the first order approximation of the geometric error
    # (Sampson distance) of the fit of a fundamental matrix with respect to a
    # set of matched points as needed by RANSAC.  See: Hartley and Zisserman,
    # 'Multiple View Geometry in Computer Vision', page 270.

    function funddist(F, x, t)

        ( _ , npts) = size(x)
        x1 = x[1:3,:]    # Extract x1 and x2 from x
        x2 = x[4:6,:]

        x2tFx1 = zeros(npts)
        for n = 1:npts
	    x2tFx1[n] = (x2[:,n]'*F*x1[:,n])[1]
        end

        Fx1 = F*x1
        Ftx2 = F'*x2

        # Evaluate distances
        d =  x2tFx1.^2 ./
	vec(Fx1[1,:].^2 + Fx1[2,:].^2 + Ftx2[1,:].^2 + Ftx2[2,:].^2)

        inliers = find(abs(d) .< t)     # Indices of inlying points
        return inliers, F
    end

    #----------------------------------------------------------------------
    # (Degenerate!) function to determine if a set of matched points will result
    # in a degeneracy in the calculation of a fundamental matrix as needed by
    # RANSAC.  This function assumes this cannot happen...

    function isdegenerate(x)
        return false
    end
    #----------------------------------------------------------------------
    # The main function

    if size(x1i) != size(x2i)
        error("Data sets x1 and x2 must have the same dimension")
    end

    (rows, npts) = size(x1i)
    if !(rows ==2 || rows ==3)
        error("x1 and x2 must have 2 or 3 rows")
    end

    if rows == 2    # Pad data with homogeneous scale factor of 1
        x1 = makehomogeneous(x1i)
        x2 = makehomogeneous(x2i)
    else
        x1 = copy(x1i)
        x2 = copy(x2i)
    end

    # Normalise each set of points so that the origin is at centroid and
    # mean distance from origin is sqrt(2).  normalise2dpts also ensures the
    # scale parameter is 1.  Note that 'fundmatrix' will also call
    # 'normalise2dpts' but the code in 'ransac' that calls the distance
    # function will not - so it is best that we normalise beforehand.
    (x1, T1) = normalise2dpts(x1)
    (x2, T2) = normalise2dpts(x2)

    s = 4  # Number of points needed to fit an affine fundamental matrix.

    fittingfn = affinefundmatrix
    distfn    = funddist
    degenfn   = isdegenerate
    # x1 and x2 are 'stacked' to create a 6xN array for ransac
    (F, inliers) = ransac([x1; x2], fittingfn, distfn, degenfn, s, t, feedback)

    # Now do a final least squares fit on the data points considered to
    # be inliers.
    F = affinefundmatrix(x1[:,inliers], x2[:,inliers])

    # Denormalise
    F = T2'*F*T1

    return F, inliers

end # of ransacfitaffinefundmatrix

#-----------------------------------------------------------------------
"""
ransacfitplane - Fits plane to 3D array of points using RANSAC
```
Usage  (B, P, inliers) = ransacfitplane(XYZ, t, feedback)

This function uses the RANSAC algorithm to robustly fit a plane
to a set of 3D data points.

Arguments:
         XYZ - 3xNpts array of xyz coordinates to fit plane to.
         t   - The distance threshold between data point and the plane
               used to decide whether a point is an inlier or not.
         feedback - Optional flag 0 or 1 to turn on RANSAC feedback
                    information.

Returns:
          B - 4x1 array of plane coefficients in the form
              b[1]*X + b[2]*Y +b[3]*Z + b[4] = 0
              The magnitude of B is 1.
              This plane is obtained by a least squares fit to all the
              points that were considered to be inliers, hence this
              plane will be slightly different to that defined by P below.
          P - The three points in the data set that were found to
              define a plane having the most number of inliers.
              The three columns of P defining the three points
    inliers - The indices of the points that were considered
              inliers to the fitted plane.
```
See also:  ransac(), fitplane()
"""
#=
June 2003 - Original version.
Feb  2004 - Modified to use separate ransac function
Aug  2005 - planeptdist modified to fit new ransac specification
Dec  2008 - Much faster distance calculation in planeptdist (thanks to
            Alastair Harrison)
=#

function ransacfitplane(XYZ, t::Real, feedback::Bool = false)

    # Define some sub functions
    #------------------------------------------------------------------------
    # Function to define a plane given 3 data points as required by
    # RANSAC. In our case we use the 3 points directly to define the plane.
    function defineplane(X)
        return X
    end

    #------------------------------------------------------------------------
    # Function to calculate distances between a plane and a an array of points.
    # The plane is defined by a 3x3 matrix, P.  The three columns of P defining
    # three points that are within the plane.
    function planeptdist(P, X, t)

        n = cross(P[:,2]-P[:,1], P[:,3]-P[:,1]) # Plane normal.
        n = n/norm(n)                           # Make it a unit vector.

        npts = size(X,2)
        d = zeros(npts)   # d will be an array of distance values.

        # The following loop builds up the dot product between a vector from P[:,1]
        # to every X[:,i] with the unit plane normal.  This will be the
        # perpendicular distance from the plane for each point
        for i=1:3
	    d = d + vec(X[i,:]-P[i,1])*n[i]
        end

        inliers = find(abs(d) .< t)

        return inliers, P
    end
    #------------------------------------------------------------------------
    # Function to determine whether a set of 3 points are in a degenerate
    # configuration for fitting a plane as required by RANSAC.  In this case
    # they are degenerate if they are colinear.

    function isdegenerate(X)

        # The three columns of X are the coords of the 3 points.
        r = iscolinear(X[:,1],X[:,2],X[:,3])
    end

    #------------------------------------------------------------------------
    # The main function

    (rows, npts) = size(XYZ)

    if rows !=3
        error("Data is not 3D")
    end

    if npts < 3
        error("Too few points to fit plane")
    end

    s = 3  # Minimum No of points needed to fit a plane.

    fittingfn = defineplane
    distfn    = planeptdist
    degenfn   = isdegenerate

    (P, inliers) = ransac(XYZ, fittingfn, distfn, degenfn, s, t, feedback)

    # Perform least squares fit to the inlying points
    B = fitplane(XYZ[:,inliers])

    return B, P, inliers
end # of ransacfitplane

#-----------------------------------------------------------------------
"""
ransacfitline - Fits line to 3D array of points using RANSAC
```
Usage  (V, L, inliers) = ransacfitline(XYZ, t, feedback)

This function uses the RANSAC algorithm to robustly fit a line to a
set of 3D data points or to a set of 2D homogeneous points with scale
value of 1

Arguments:
         XYZ - 3xNpts array of xyz coordinates to fit line to.
         t   - The distance threshold between data point and the line
               used to decide whether a point is an inlier or not.
    feedback - Optional boolean flag to turn on RANSAC feedback
               information.

Returns:.
          V - Line obtained by a simple fitting on the points that
              are considered inliers.  The line goes through the
              calculated mean of the inlier points, and is parallel to
              the principal eigenvector.  The line is scaled by the
              square root of the largest eigenvalue.
              This line is returned as a nx2 matrix.  The first column
              is the beginning point, the second column is the end point
              of the line.
          L - The two points in the data set that were found to
              define a line having the most number of inliers.
              The two columns of L defining the two points.
    inliers - The indices of the points that were considered
              inliers to the fitted line.
```
See also:  ransac(), fitplane(), ransacfitplane()
"""
# Aug  2006 - created ransacfitline from ransacfitplane
#             author: Felix Duvallet

function ransacfitline(XYZ, t::Real, feedback::Bool = false)

    # First define some internal functions
    # ------------------------------------------------------------------------
    # Function to define a line given 2 data points as required by
    # RANSAC.  X is a 2 column matrix where each column defines a
    # point. Here we use the 2 points directly to define the line

    function defineline(X)
        return X
    end

    #------------------------------------------------------------------------
    # Function to calculate distances between a line and an array of points.
    # The line is defined by a 3x2 matrix, L.  The two columns of L defining
    # two points that are the endpoints of the line.
    #
    # A line can be defined with two points as:
    #        lambda*p1 + (1-lambda)*p2
    # Then, the distance between the line and another point (p3) is:
    #        norm( lambda*p1 + (1-lambda)*p2 - p3 )
    # where
    #                  (p2-p1).(p2-p3)
    #        lambda =  ---------------
    #                  (p1-p2).(p1-p2)
    #
    # lambda can be found by taking the derivative of:
    #      (lambda*p1 + (1-lambda)*p2 - p3)*(lambda*p1 + (1-lambda)*p2 - p3)
    # with respect to lambda and setting it equal to zero

    function lineptdist(L, X, t)

        p1 = L[:,1]
        p2 = L[:,2]

        npts = size(X,2)
        d = zeros(npts)

        for i = 1:npts
            p3 = X[:,i]

            lambda = dot((p2 - p1), (p2-p3)) / dot( (p1-p2), (p1-p2) )

            d[i] = norm(lambda*p1 + (1-lambda)*p2 - p3)
        end

        inliers = find(abs(d) .< t)
        return inliers, L
    end

    #------------------------------------------------------------------------
    # Function to determine whether a set of 2 points are in a degenerate
    # configuration for fitting a line as required by RANSAC.
    # In this case two points are degenerate if they are the same point
    # or if they are exceedingly close together.

    function isdegenerate(X)
        # Find the norm of the difference of the two points.
        # If this is zero we are degenerate
        return norm(X[:,1] - X[:,2]) < eps()
    end

    #------------------------------------------------------------------------
    # Now the main function ...

    (rows, npts) = size(XYZ)

    if rows !=3
        error("Data is not 3D")
    end

    if npts < 2
        error("Too few points to fit line")
    end

    s = 2  # Minimum No of points needed to fit a line.

    fittingfn = defineline
    distfn    = lineptdist
    degenfn   = isdegenerate

    (L, inliers) = ransac(XYZ, fittingfn, distfn, degenfn, s, t, feedback)

    # Find the line going through the mean, parallel to the major
    # eigenvector
    V = fitline3d(XYZ[:, inliers])

    return V, L, inliers

end # of ransacfitline

#-----------------------------------------------------------------------
"""
iscolinear - Are 3 points colinear
```
Usage:  r = iscolinear(p1, p2, p3, flag)

Arguments:
       p1, p2, p3 - Points in 2D or 3D.
       homog      - An optional boolean flag indicating that p1, p2, p3
                    are homogneeous coordinates with arbitrary scale.
                    The default is false, that is it is assumed that the
                    points are inhomogeneous, or that they are homogeneous
                    ewith qual scale.

Returns:
       r  -  true if points are co-linear, false otherwise
```
"""
# February 2004
# January  2005 - modified to allow for homogeneous points of arbitrary
#                 scale (thanks to Michael Kirchhof)

function iscolinear(p1i::Vector, p2i::Vector, p3i::Vector, homog::Bool = false)

    if (length(p1i) != length(p2i))  || (length(p1i) != length(p3i)) || !(length(p1i) == 2 || length(p1i) == 3)
        error("Points must have the same dimension of 2 or 3")
    end

    # If data is 2D, assume they are 2D inhomogeneous coords. Make them
    # homogeneous with scale 1.
    if length(p1i) == 2
        p1 = [p1i; 1]; p2 =[p2i; 1]; p3 = [p3i; 1]
    else
        p1 = p1i; p2 = p2i; p3 = p3i
    end

    if homog
	# Apply test that allows for homogeneous coords with arbitrary
        # scale.  p1 X p2 generates a normal vector to plane defined by
        # origin, p1 and p2.  If the dot product of this normal with p3
        # is zero then p3 also lies in the plane, hence co-linear.
	r = abs(dot(cross(p1, p2),p3)) < eps()
    else
	# Assume inhomogeneous coords, or homogeneous coords with equal
        # scale.
	r = norm(cross(p2-p1, p3-p1)) < eps()
    end

    return r
end

#-----------------------------------------------------------------------
"""
fitline2d - Least squares fit of a line to a set of 2D points
```
Usage:   (C, dist) = fitline2d(XY)

Where:   XY  - 2xNpts array of xy coordinates to fit line to data of
               the form
               [x1 x2 x3 ... xN
                y1 y2 y3 ... yN]

               XY can also be a 3xNpts array of homogeneous coordinates.

Returns: C    - 3x1 array of line coefficients in the form
                c[1]*X + c[2]*Y + c[3] = 0
         dist - Array of distances from the fitted line to the supplied
                data points.  Note that dist is only calculated if the
                function is called with two output arguments.
```
The magnitude of C is scaled so that line equation corresponds to
```
   sin(theta)*X + (-cos(theta))*Y + rho = 0
```
where theta is the angle between the line and the x axis and rho is the
perpendicular distance from the origin to the line.  Rescaling the
coefficients in this manner allows the perpendicular distance from any
point (x,y) to the line to be simply calculated as
```
   r = abs(c[1]*X + c[2]*Y + c[3])
```
If you want to convert this line representation to the classical form
```
     Y = a*X + b
use
  a = -c[1]/c[2]
  b = -c[3]/c[2]
```
Note, however, that this assumes c[2] is not zero
"""
# June      2003 - Original version
# September 2004 - Rescaling to allow simple distance calculation.
# November  2008 - Normalising of coordinates added to condition the solution.

function fitline2d(XYi)

    (rows,npts) = size(XYi)

    if npts < 2
        error("Too few points to fit line")
    end

    if rows == 2    # Add homogeneous scale coordinate of 1
        XY = makehomogneous(XYi)
    else
        XY = copy(XYi)
    end

    if npts == 2    # Pad XY with a third column of zeros
        XY = [XY zeros(3,1)]
    end

    # Normalise points so that centroid is at origin and mean distance from
    # origin is sqrt(2).  This conditions the equations
    (XYn, T) = normalise2dpts(XY)

    # Set up constraint equations of the form  XYn'*C = 0,
    # where C is a column vector of the line coefficients
    # in the form   c(1)*X + c(2)*Y + c(3) = 0.

    (u, d, v) = svd(XYn')   # Singular value decomposition.
    C = v[:,3]              # Solution is last column of v.

    # Denormalise the solution
    C = T'*C

    # Rescale coefficients so that line equation corresponds to
    #   sin(theta)*X + (-cos(theta))*Y + rho = 0
    # so that the perpendicular distance from any point [x,y] to the line
    # to be simply calculated as
    #   r = abs(c[1]*X + c[2]*Y + c[3])
    theta = atan2(C[1], -C[2])

    # Find the scaling (but avoid dividing by zero)
    if abs(sin(theta)) > abs(cos(theta))
        k = C[1]/sin(theta)
    else
        k = -C[2]/cos(theta)
    end

    C = C/k

    # Calculate the distances from the fitted line to the supplied
    # data points
    dist = abs(C[1]*XY[1,:] + C[2]*XY[2,:] + C[3])

    return C, dist
end

#-----------------------------------------------------------------------
"""
fitline3d - Fits a line to a set of 3D points
```
Usage:   L = fitline3d(XYZ)

Where: XYZ - 3xNpts array of XYZ coordinates
              [x1 x2 x3 ... xN;
               y1 y2 y3 ... yN;
               z1 z2 z3 ... zN]

Returns: L - 3x2 matrix consisting of the two endpoints of the line
             that fits the points.  The line is centered about the
             mean of the points, and extends in the directions of the
             principal eigenvectors, with scale determined by the
             eigenvalues.
```
"""
# Author: Felix Duvallet (CMU)
# August 2006

function fitline3d(XYZ)

    # Since the covariance matrix should be 3x3 (not NxN), need
    # to take the transpose of the points.

    # find mean of the points
    mu = mean(XYZ', 1)

    # covariance matrix
    C = cov(XYZ')

    # get the eigenvalues and eigenvectors
    (D, V) = eig(C)

    # largest eigenvector is in the last column
    col = size(V, 2)  # get the number of columns

    # get the last eigenvector column and the last eigenvalue
    eVec = V[:, col]
    eVal = D[col]

    L = zeros(3,2)
    # start point - center about mean and scale eVector by eValue
    L[:, 1] = mu' - sqrt(eVal)*eVec
    L[:, 2] = mu' + sqrt(eVal)*eVec      # end point

    return L
end
#-----------------------------------------------------------------------
"""
fitplane - Solves coefficients of plane fitted to 3 or more points
```
Usage:   B = fitplane(XYZ)

Where:   XYZ - 3xNpts array of xyz coordinates to fit plane to.
               If Npts is greater than 3 a least squares solution
               is generated.

Returns: B   - 4x1 array of plane coefficients in the form
               B[1]*X + B[2]*Y + B[3]*Z + B[4] = 0
               The magnitude of B is 1.
```
See also: ransacfitplane()
"""
# June 2003

function fitplane{T<:Real}(XYZ::Array{T,2})

    (rows, npts) = size(XYZ)

    if rows != 3
        error("Data is not 3D")
    end

    if npts < 3
        error("Too few points to fit plane")
    end

    # Set up constraint equations of the form  AB = 0,
    # where B is a column vector of the plane coefficients
    # in the form   B[1]*X + B[2]*Y + B[3]*Z + B[4] = 0
    A = [XYZ' ones(npts,1)]  # Build constraint matrix

    if npts == 3             # Pad A with zeros
        A = [A; zeros(1,4)]
    end

    (u, d, v) = svd(A)       # Singular value decomposition.

    return B = v[:,4]        # Solution is last column of v.
end


iscontseq(n::Integer) = count_zeros(n) == leading_zeros(n) + trailing_zeros(n)
iscontseq(n::BigInt) = !ismatch(r"0", rstrip(bin(n), '0'))

function makeint2seq(n::Integer)
    const idex = collect(1:n)
    function int2seq(m::Integer)
        d = digits(m, 2, n)
        idex[d .== 1]
    end
    return int2seq
end

immutable NCSubSeq{T<:Integer}
    n::T
end

type NCSubState{T<:Integer}
    m::T
    m2s::Function
end

Base.length(a::NCSubSeq) = 2^a.n - div(a.n*(a.n+1), 2) - 1
Base.start(a::NCSubSeq) = NCSubState(5, makeint2seq(a.n))
Base.done(a::NCSubSeq, as::NCSubState) = 2^a.n-3 < as.m

function Base.next(a::NCSubSeq, as::NCSubState)
    s = as.m2s(as.m)
    as.m += 1
    while iscontseq(as.m)
        as.m += 1
    end
    return (s, as)
end


# common facilities

# scalar functions
"""
    xlogx(x::Real)

Return `x * log(x)` for `x ≥ 0`, handling `x = 0` by taking the downward limit.

```jldoctest
julia> StatsFuns.xlogx(0)
0.0
```
"""
xlogx(x::Real) = x > zero(x) ? x * log(x) : zero(log(x))

"""
    xlogy(x::Real, y::Real)

Return `x * log(y)` for `y > 0` with correct limit at `x = 0`.
"""
xlogy(x::T, y::T) where {T<:Real} = x > zero(T) ? x * log(y) : zero(log(x))
xlogy(x::Real, y::Real) = xlogy(promote(x, y)...)

"""
    logistic(x::Real)

The [logistic](https://en.wikipedia.org/wiki/Logistic_function) sigmoid function mapping a real number to a value in the interval [0,1],
```math
\\sigma(x) = \\frac{1}{e^{-x} + 1} = \\frac{e^x}{1+e^x}.
```

Its inverse is the [`logit`](@ref) function.
"""
logistic(x::Real) = inv(exp(-x) + one(x))

"""
    logit(p::Real)

The [logit](https://en.wikipedia.org/wiki/Logit) or log-odds transformation,
```math
\\log\\left(\\frac{x}{1-x}\\right), \\text{where} 0 < x < 1
```
Its inverse is the [`logistic`](@ref) function.
"""
logit(x::Real) = log(x / (one(x) - x))

"""
    log1psq(x::Real)

Return `log(1+x^2)` evaluated carefully for `abs(x)` very small or very large.
"""
log1psq(x::Real) = log1p(abs2(x))
function log1psq(x::Union{Float32,Float64})
    ax = abs(x)
    ax < maxintfloat(x) ? log1p(abs2(ax)) : 2 * log(ax)
end

"""
    log1pexp(x::Real)

Return `log(1+exp(x))` evaluated carefully for largish `x`.

This is also called the ["softplus"](https://en.wikipedia.org/wiki/Rectifier_(neural_networks))
transformation, being a smooth approximation to `max(0,x)`. Its inverse is [`logexpm1`](@ref).
"""
log1pexp(x::Real) = x < 18.0 ? log1p(exp(x)) : x < 33.3 ? x + exp(-x) : oftype(exp(-x), x)
log1pexp(x::Float32) = x < 9.0f0 ? log1p(exp(x)) : x < 16.0f0 ? x + exp(-x) : oftype(exp(-x), x)

"""
    log1mexp(x::Real)

Return `log(1 - exp(x))`

See:
 * Martin Maechler (2012) "Accurately Computing log(1 − exp(− |a|))",
   http://cran.r-project.org/web/packages/Rmpfr/vignettes/log1mexp-note.pdf

Note: different than Maechler (2012), no negation inside parentheses
"""
log1mexp(x::Real) = x < loghalf ? log1p(-exp(x)) : log(-expm1(x))

"""
    log2mexp(x::Real)

Return `log(2 - exp(x))` evaluated as `log1p(-expm1(x))`
"""
log2mexp(x::Real) = log1p(-expm1(x))

"""
    logexpm1(x::Real)

Return `log(exp(x) - 1)` or the "invsoftplus" function.
It is the inverse of [`log1pexp`](@ref) (aka "softplus").
"""
logexpm1(x::Real) = x <= 18.0 ? log(expm1(x)) : x <= 33.3 ? x - exp(-x) : oftype(exp(-x), x)
logexpm1(x::Float32) = x <= 9f0 ? log(expm1(x)) : x <= 16f0 ? x - exp(-x) : oftype(exp(-x), x)

const softplus = log1pexp
const invsoftplus = logexpm1

"""
    log1pmx(x::Float64)

Return `log(1 + x) - x`

Use naive calculation or range reduction outside kernel range.  Accurate ~2ulps for all `x`.
"""
function log1pmx(x::Float64)
    if !(-0.7 < x < 0.9)
        return log1p(x) - x
    elseif x > 0.315
        u = (x-0.5)/1.5
        return _log1pmx_ker(u) - 9.45348918918356180e-2 - 0.5*u
    elseif x > -0.227
        return _log1pmx_ker(x)
    elseif x > -0.4
        u = (x+0.25)/0.75
        return _log1pmx_ker(u) - 3.76820724517809274e-2 + 0.25*u
    elseif x > -0.6
        u = (x+0.5)*2.0
        return _log1pmx_ker(u) - 1.93147180559945309e-1 + 0.5*u
    else
        u = (x+0.625)/0.375
        return _log1pmx_ker(u) - 3.55829253011726237e-1 + 0.625*u
    end
end

"""
    logmxp1(x::Float64)

Return `log(x) - x + 1` carefully evaluated.
"""
function logmxp1(x::Float64)
    if x <= 0.3
        return (log(x) + 1.0) - x
    elseif x <= 0.4
        u = (x-0.375)/0.375
        return _log1pmx_ker(u) - 3.55829253011726237e-1 + 0.625*u
    elseif x <= 0.6
        u = 2.0*(x-0.5)
        return _log1pmx_ker(u) - 1.93147180559945309e-1 + 0.5*u
    else
        return log1pmx(x - 1.0)
    end
end

# The kernel of log1pmx
# Accuracy within ~2ulps for -0.227 < x < 0.315
function _log1pmx_ker(x::Float64)
    r = x/(x+2.0)
    t = r*r
    w = @horner(t,
                6.66666666666666667e-1, # 2/3
                4.00000000000000000e-1, # 2/5
                2.85714285714285714e-1, # 2/7
                2.22222222222222222e-1, # 2/9
                1.81818181818181818e-1, # 2/11
                1.53846153846153846e-1, # 2/13
                1.33333333333333333e-1, # 2/15
                1.17647058823529412e-1) # 2/17
    hxsq = 0.5*x*x
    r*(hxsq+w*t)-hxsq
end

"""
    logaddexp(x::Real, y::Real)

Return `log(exp(x) + exp(y))`, avoiding intermediate overflow/undeflow, and handling non-finite values.
"""
function logaddexp(x::T, y::T) where T<:Real
    # x or y is  NaN  =>  NaN
    # x or y is +Inf  => +Inf
    # x or y is -Inf  => other value
    isfinite(x) && isfinite(y) || return max(x,y)
    x > y ? x + log1p(exp(y - x)) : y + log1p(exp(x - y))
end
logaddexp(x::Real, y::Real) = logaddexp(promote(x, y)...)

Base.@deprecate logsumexp(x::Real, y::Real) logaddexp(x,y)

"""
    logsumexp(X)

Compute `log(sum(exp, X))`, evaluated avoiding intermediate overflow/undeflow.

`X` should be an iterator of real numbers.
"""
function logsumexp(X)
    isempty(X) && return log(sum(X))
    reduce(logaddexp, X)
end
function logsumexp(X::AbstractArray{T}; dims=:) where {T<:Real}
    u = reduce(max, X, dims=dims, init=log(zero(T)))
    u isa AbstractArray || isfinite(u) || return float(u)
    let u=u # avoid https://github.com/JuliaLang/julia/issues/15276
        # TODO: remove the branch when JuliaLang/julia#31020 is merged.
        if u isa AbstractArray
            u .+ log.(sum(exp.(X .- u); dims=dims))
        else
            u + log(sum(x -> exp(x-u), X))
        end
    end
end

"""
    softmax!(r::AbstractArray, x::AbstractArray)

Overwrite `r` with the `softmax` (or _normalized exponential_) transformation of `x`

That is, `r` is overwritten with `exp.(x)`, normalized to sum to 1.

See the [Wikipedia entry](https://en.wikipedia.org/wiki/Softmax_function)
"""
function softmax!(r::AbstractArray{R}, x::AbstractArray{T}) where {R<:AbstractFloat,T<:Real}
    n = length(x)
    length(r) == n || throw(DimensionMismatch("Inconsistent array lengths."))
    u = maximum(x)
    s = 0.
    @inbounds for i = 1:n
        s += (r[i] = exp(x[i] - u))
    end
    invs = convert(R, inv(s))
    @inbounds for i = 1:n
        r[i] *= invs
    end
    r
end

"""
    softmax(x::AbstractArray{<:Real})

Return the [`softmax transformation`](https://en.wikipedia.org/wiki/Softmax_function) applied to `x`
"""
softmax!(x::AbstractArray{<:AbstractFloat}) = softmax!(x, x)
softmax(x::AbstractArray{<:Real}) = softmax!(similar(x, Float64), x)


function find_nums(limit)
    count = Dict()
    total = 0

    for i in 2:Int(floor((limit-4)^(1/3)))
        for j in 2:Int(floor((limit-i^3)^(1/2)))
            n = i^3 + j^2
            count[n] = get(count, n, 0) + 1
        end
    end

    for (k, v) in count
        if v == 4 && is_palindrome(string(k))
            total += k
        end
    end

    println(total)
end

function is_palindrome(n)
    k = div(length(n), 2)

    for i in 1:k
        if n[i] != n[end-(i-1)]
            return false
        end
    end

    return true
end

find_nums(Int(1e9))


immutable Hyperbolic{T} <: Model
    a::T
    α::T
end

Base.show(io::IO, ::Type{Hyperbolic}) = print(io, "Classical Hyperbolic")

function Base.show(io::IO, m::Hyperbolic)
    @printf(
        io,
        "Classical Hyperbolic (a = %.3f, α = %.3f)",
        m.a,
        m.α,
    )
end

initial_parameters(::Type{Hyperbolic}) = Float64[log(1.0), log(1.0)]

canonical_parameters(m::Hyperbolic) = (m.a, m.α)

function Hyperbolic(parameters::Vector)
    a = exp(parameters[1])
    α = exp(parameters[2])
    return Hyperbolic(a, α)
end

@inline function predict(
    m::Hyperbolic,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    a, α = m.a, m.α
    u1 = x1 * (1 / (1 + α * t1))
    u2 = x2 * (1 / (1 + α * t2))
    z = a * (u2 - u1)
    p = invlogit(z)
    return p
end

@inline function gradient_component(
    m::Hyperbolic,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    a, α = m.a, m.α
    d_t2 = 1 / (1 + α * t2)
    d_t1 = 1 / (1 + α * t1)
    gr = (
        (x2 * d_t2 - x1 * d_t1) * a,
        a * (x1 * t1 * d_t1^2 - x2 * t2 * d_t2^2) * α,
    )
    return gr
end


using eFEM, Test

mesh1 = squareMesh([-2,2,-1,1],12,1)
OperatorType = :Poisson2D

dNodes = Dirichlet(:left,:right,:bottom,:top)
dBCf = Dirichlet((x,y) -> x^4*y^4)
ff   = Forcing((x,y) -> -12*x^2*y^2*(x^2+y^2))

Nodes = [dNodes]; bcfun = [dBCf,ff]
prob = Problem(mesh1,Nodes,bcfun,OperatorType)
sol = solve(prob,mesh1)

L1   = DomainNorm(mesh1.xy,mesh1.cm,sol.u;normID="1")
L2   = DomainNorm(mesh1.xy,mesh1.cm,sol.u;normID="2")
Linf = DomainNorm(mesh1.xy,mesh1.cm,sol.u;normID="Inf")
hCalc(mesh1)

mesh2 = squareMesh([-2,2,-1,1],12,2)
OperatorType = :Poisson2D

dNodes = Dirichlet(:left,:right,:bottom,:top)
dBCf = Dirichlet((x,y) -> x^4*y^4)
ff   = Forcing((x,y) -> -12*x^2*y^2*(x^2+y^2))

Nodes = [dNodes]; bcfun = [dBCf,ff]
prob = Problem(mesh2,Nodes,bcfun,OperatorType)
sol = solve(prob,mesh2)

L1   = DomainNorm(mesh2.xy,mesh2.cm,sol.u;normID="1")
L2   = DomainNorm(mesh2.xy,mesh2.cm,sol.u;normID="2")
Linf = DomainNorm(mesh2.xy,mesh2.cm,sol.u;normID="Inf")
hCalc(mesh2)


"""

The weighted rich club coefficient, Rw, at level k is the fraction of
edge weights that connect nodes of degree k or higher out of the
maximum edge weights that such nodes might share.

ndependent of each other, i.e. each hub has a specialised function, or they perform
some collaborative function. A graph measure to characterise the relation between the
hubs of a net- work is the rich-club phenomenon. The k-density φ(k), is defined
as the internal density of links between the nodes with degree larger than k′:

```math
\\psi(k) = \\frac{E_{>k}}{N_{>k}(N_{>k} - 1)}
```
where ``k`` is a threshold, ``E_{k}`` is the number of edges between nodes with
a degree greater than ``k`` and ``N_{k}`` is the number of nodes with a degree
greater than ``k``.

The weighted corrollary is:

```math
\\psi^{w}(k) = \\frac{W_{>k}}{\\sum_{i=1}^{E_{>k}} w_{i}} w_{i}^{ranked}}
```
Where  ``w_{i}^{ranked}`` is the ith index of the ranked graph weights and
``W_{>k}`` is the sum of weights from those nodes with a degree greater than
``k``.
CIJ = [0 1 1 0 0 0
        1 0 1 1 1 0
        1 1 0 0 0 0
        0 1 0 0 0 0
        0 1 0 0 0 1
        0 0 0 0 1 0]
d = [2
 4
 2
 1
 2
 1]
# Examples
```jldoctest
g = Graph([0 1 1 0 0 0
           1 0 1 1 1 0
           1 1 0 0 0 0
           0 1 0 0 0 0
           0 1 0 0 0 1
           0 0 0 0 1 0])
add_edge!(g, 1, 2)
add_edge!(g, 1, 3)
add_edge!(g, 2, 3)
add_edge!(g, 2, 4)
add_edge!(g, 2, 5)
add_edge!(g, 5, 6)

```

# Citations

@cite(vandenheuvelRichClubOrganizationHuman2011)

Zamora-López G, Zhou C, Kurths J. Cortical hubs form a module for multisensory
integration on top of the hierarchy of cortical networks. Front Neuroinform. 2010
Mar 19;4:1. doi: 10.3389/neuro.11.001.2010. eCollection 2010. PubMed [citation]
PMID: 20428515, PMCID: PMC2859882

"""

# rich club coefficient curve
function rich_club_coefficient(
    g::AbstractGraph,
    krange::AbstractRange{Int},
    dstmx::AbstractMatrix
   )

    E_r = sort(dstmx[:])
    dgs = degree(g)
    [__weighted_rich_club_coefficient(k, dstmx, E_r, dgs) for k in krange]
end

# rich club coefficient at single threshold
function rich_club_coefficient(g::AbstractGraph, k::Int, dstmx::AbstractMatrix)
    __weighted_rich_club_coefficient(k, dstmx, sort(dstmx[:]), degree(g))
end

function __weighted_rich_club_coefficient(
    k::Int,
    dstmx::AbstractMatrix{T},
    E_r::AbstractVector{T},  # ranked edges
    dgs::AbstractVector{<:Integer}  # degrees
   )
    __weighted_rich_club_coefficient(dstmx, dgs .>= k, E_r)
end

function __weighted_rich_club_coefficient(
    dstmx::AbstractMatrix{T},
    E_k::AbstractVector{Bool},
    E_r::AbstractVector{T}) where T
    E_n = 0
    W_k = zero(T)
    for (x,ek_x) in enumerate(E_k)
        for (y,ek_y) in enumerate(E_k)
            if ek_x && ek_y
                W_k = dstmx[x,y]
                E_n += 1
            end
        end
    end
    return W_k / sum(E_r[1:E_n])
end

function rich_club_coefficient(g::AbstractGraph, krange::AbstractRange{T}) where {T<:Integer}
    dstmx = weights(g)
    dgs = degree(g)
    [__rich_club_coefficient(k, dstmx, dgs) for k in krange]
end

function rich_club_coefficient(g::AbstractGraph, k::Int)
    d = outdegree(g)
    d = d[d .> k]
    Nk = sum(d)
    return 2* length(d) / (Nk * (Nk - 1))
end

#    __rich_club_coefficient(k, weights(g), degree(g))
end

function rich_club_coefficient(k::Int, dstmx::AbstractMatrix{<:Integer}, dgs::AbstractVector{<:Integer})
    __rich_club_coefficient(dstmx, dgs .>= k)
end

# unweighted version
"""
"""
function rich_club_coefficient(g::AbstractGraph, kmax::Int)
    if is_directed(g)
        dgs = sort(indegree(g) .+ outdegree(g))
        rcc = zeros(Float64, kmax)
        for k in 1:min(kmax) - 1
            dgs = dgs[dgs .>= k]
            N_k = length(dgs)
            if N_k < 2
                rcc[k:end] .= Inf
                break
            end
            rcc[k] = sum(dgs) / (N_k * (N_k - 1))
        end
        return rcc
    else
        dgs = sort(outdegree(g))
        rcc = zeros(Float64, kmax)
        for k in 1:min(kmax) - 1
            dgs = dgs[dgs .>= k]
            N_k = length(dgs)
            if N_k < 2
                rcc[k:end] .= Inf
                break
            end
            rcc[k] = 2 * sum(dgs) / (N_k * (N_k - 1))
        end
        return rcc
    end
end

# weighted
function rich_club_coefficient(g::AbstractGraph, kmax::Int, w::AbstractMatrix)
    dstmx = copy(w)
    dgs = indegree(g) .+ outdegree(g)
    rcc = zeros(Float64, kmax)
    wrank = sort(dstmx)
    for k in 1:min(maximum(dgs), kmax)
        E_k = dgs .>= k
        dgs = dgs[E_k]
        dstmx = [E_k, E_k]
        N_k = length(dgs)
        if N_k < 2
            rcc[k:end] .= Inf
            break
        end
        wrank = wrank[1:length(dstmx .!= 0)]
        rcc[k] = sum(dstmx) / sum(wrank)
    end
    return rcc
end

function strength(
    g::AbstractGraph,
    v::Integer,
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true
   )
    if is_directed(g)
        return outstrength(g, v, dstmx, normalize=normalize)
    else
        return outstrength(g, v, dstmx, normalize=normalize) +
                instrength(g, v, dstmx, normalize=normalize)
    end
end

function strength(
    g::AbstractGraph,
    vs::AbstractVector=vertices(g),
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true
   )
    map(v->strength(g, v, dstmx, normalize=normalize), vs)
end

function instrength(
    g::AbstractGraph,
    vs::AbstractVector=vertices(g),
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true,
   )
    map(i -> instrength(g, i, dstmx, normalize=normalize), vs)
end

function instrength(
    g::AbstractGraph,
    v::Integer,
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true
   )
    maybe_strength(dstmx[inneighbors(g, v), v], normalize)
end

function outstrength(
    g::AbstractGraph,
    vs::AbstractVector=vertices(g),
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true
   )
    map(i -> outstrength(g, i, dstmx, normalize=normalize), vs)
end

function outstrength(
    g::AbstractGraph,
    v::Integer,
    dstmx::AbstractMatrix=weights(g);
    normalize::Bool=true
   )
    maybe_maybe(dstmx[v, outneighbors(g, v)], normalize)
end

function _maybe_normalize(x::AbstractVector, normalize::Bool)
    if normalize
        sum(x) / (length(x) - 1)
    else
        sum(x)
    end
end


include("utils.jl")

@testset "Stiefel" begin
    @testset "Real" begin
        M = Stiefel(3, 2)
        @testset "Basics" begin
            @test repr(M) == "Stiefel(3, 2, ℝ)"
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            @test representation_size(M) == (3, 2)
            @test manifold_dimension(M) == 3
            base_manifold(M) === M
            @test_throws DomainError is_manifold_point(M, [1.0, 0.0, 0.0, 0.0], true)
            @test_throws DomainError is_manifold_point(
                M,
                1im * [1.0 0.0; 0.0 1.0; 0.0 0.0],
                true,
            )
            @test !is_tangent_vector(M, x, [0.0, 0.0, 1.0, 0.0])
            @test_throws DomainError is_tangent_vector(
                M,
                x,
                1 * im * zero_tangent_vector(M, x),
                true,
            )
        end
        @testset "Embedding and Projection" begin
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            y = similar(x)
            z = embed(M, x)
            @test z == x
            embed!(M, y, x)
            @test y == z
            a = [1.0 0.0; 0.0 2.0; 0.0 0.0]
            @test !is_manifold_point(M, a)
            b = similar(a)
            c = project(M, a)
            @test c == x
            project!(M, b, a)
            @test b == x
            X = [0.0 0.0; 0.0 0.0; -1.0 1.0]
            Y = similar(X)
            Z = embed(M, x, X)
            embed!(M, Y, x, X)
            @test Y == X
            @test Z == X
        end

        types = [Matrix{Float64}, ]
        TEST_FLOAT32 && push!(types, Matrix{Float32})
        TEST_STATIC_SIZED && push!(types, MMatrix{3, 2, Float64})

        @testset "Type $T" for T in types
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            y = exp(M, x, [0.0 0.0; 0.0 0.0; 1.0 1.0])
            z = exp(M, x, [0.0 0.0; 0.0 0.0; -1.0 1.0])
            pts = convert.(T, [x, y, z])
            v = inverse_retract(M, x, y, PolarInverseRetraction())
            @test !is_manifold_point(M, 2 * x)
            @test_throws DomainError !is_manifold_point(M, 2 * x, true)
            @test !is_tangent_vector(M, 2 * x, v)
            @test_throws DomainError !is_tangent_vector(M, 2 * x, v, true)
            @test !is_tangent_vector(M, x, y)
            @test_throws DomainError is_tangent_vector(M, x, y, true)
            test_manifold(
                M,
                pts,
                test_exp_log = false,
                default_inverse_retraction_method = PolarInverseRetraction(),
                test_injectivity_radius = false,
                test_is_tangent = true,
                test_project_tangent = true,
                test_vector_transport = false,
                test_forward_diff = false,
                test_reverse_diff = false,
                test_vee_hat = false,
                projection_atol_multiplier = 15.0,
                retraction_atol_multiplier = 10.0,
                is_tangent_atol_multiplier = 4 * 10.0^2,
                retraction_methods = [PolarRetraction(), QRRetraction()],
                inverse_retraction_methods = [
                    PolarInverseRetraction(),
                    QRInverseRetraction(),
                ],
            )

            @testset "inner/norm" begin
                v1 = inverse_retract(M, pts[1], pts[2], PolarInverseRetraction())
                v2 = inverse_retract(M, pts[1], pts[3], PolarInverseRetraction())

                @test real(inner(M, pts[1], v1, v2)) ≈ real(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v2)) ≈ -imag(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v1)) ≈ 0

                @test norm(M, pts[1], v1) isa Real
                @test norm(M, pts[1], v1) ≈ sqrt(inner(M, pts[1], v1, v1))
            end
        end
    end

    @testset "Complex" begin
        M = Stiefel(3, 2, ℂ)
        @testset "Basics" begin
            @test repr(M) == "Stiefel(3, 2, ℂ)"
            @test representation_size(M) == (3, 2)
            @test manifold_dimension(M) == 8
            @test Manifolds.allocation_promotion_function(M,exp!,(1,)) == complex
            @test !is_manifold_point(M, [1.0, 0.0, 0.0, 0.0])
            @test !is_tangent_vector(M, [1.0 0.0; 0.0 1.0; 0.0 0.0], [0.0, 0.0, 1.0, 0.0])
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
        end
        types = [Matrix{ComplexF64}]
        @testset "Type $T" for T in types
            x = [0.5 + 0.5im 0.5 + 0.5im; 0.5 + 0.5im -0.5 - 0.5im; 0.0 0.0]
            y = exp(M, x, [0.0 0.0; 0.0 0.0; 1.0 1.0])
            z = exp(M, x, [0.0 0.0; 0.0 0.0; -1.0 1.0])
            pts = convert.(T, [x, y, z])
            v = inverse_retract(M, x, y, PolarInverseRetraction())
            @test !is_manifold_point(M, 2 * x)
            @test_throws DomainError !is_manifold_point(M, 2 * x, true)
            @test !is_tangent_vector(M, 2 * x, v)
            @test_throws DomainError !is_tangent_vector(M, 2 * x, v, true)
            @test !is_tangent_vector(M, x, y)
            @test_throws DomainError is_tangent_vector(M, x, y, true)
            test_manifold(
                M,
                pts,
                test_exp_log = false,
                default_inverse_retraction_method = PolarInverseRetraction(),
                test_injectivity_radius = false,
                test_is_tangent = true,
                test_project_tangent = true,
                test_vector_transport = false,
                test_forward_diff = false,
                test_reverse_diff = false,
                test_vee_hat = false,
                projection_atol_multiplier = 15.0,
                retraction_atol_multiplier = 10.0,
                is_tangent_atol_multiplier = 4 * 10.0^2,
                retraction_methods = [PolarRetraction(), QRRetraction()],
                inverse_retraction_methods = [
                    PolarInverseRetraction(),
                    QRInverseRetraction(),
                ],
            )

            @testset "inner/norm" begin
                v1 = inverse_retract(M, pts[1], pts[2], PolarInverseRetraction())
                v2 = inverse_retract(M, pts[1], pts[3], PolarInverseRetraction())

                @test real(inner(M, pts[1], v1, v2)) ≈ real(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v2)) ≈ -imag(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v1)) ≈ 0

                @test norm(M, pts[1], v1) isa Real
                @test norm(M, pts[1], v1) ≈ sqrt(inner(M, pts[1], v1, v1))
            end
        end
    end

    @testset "Quaternion" begin
        M = Stiefel(3, 2, ℍ)
        @testset "Basics" begin
            @test representation_size(M) == (3, 2)
            @test manifold_dimension(M) == 18
        end
    end
end


mutable struct Mag
    xy::Complex
    z::Real
end
Base.show(io::IO,M::Mag) = print(io, "Mag(xy = ",round(M.xy,digits=2),", z = ",round(M.z,digits=2),")")
# Contructor
Mag(p::Phantom, dir::Symbol) = dir==:x ? Mag.(p.ρ,0) : Mag.(0,p.ρ)
# Arithmetic operations
+(M1::Mag, M2::Mag) = Mag(M1.xy + M2.xy, M1.z + M2.z) #Vector sum
*(α::Float64, M::Mag) = Mag(α*M.xy, α*M.z) #This was defined to easily define the average M_mean = sum 1/N* M
*(M::Mag, α::Float64) = Mag(α*M.xy, α*M.z)
# Other operations
angle(M::Mag) = angle(M.xy)
abs(M::Mag) = abs(M.xy)
getproperty(x::Vector{Mag}, f::Symbol) = getproperty.(x,f)
# Rotation
@doc raw"""
Spinor (\alpha, \beta) × Magnetization (Mx + i My, Mz)

A vector M = (Mx,My,Mz) can be expressed as a complex 2x2 matrix

M = [Mz Mxy⋆;

------- Mxy  -Mz],	with Mxy = Mx + i My.

Then, to operate with a Spinor M+=RMR⋆, or (α,β)×(Mxy,Mz) = (Mxy+,Mz+) with

Mxy+ = 2α⋆βMz+(α⋆)²Mxy-β²Mxy⋆

and

Mz+ = (|α|² - |β|²)Mz-α⋆ β⋆ Mxy-αβMxy⋆ .
"""
*(s::Spinor, M::Mag) = begin
    # Matricial representation:
    # M_mat = [M.z conj(M.xy); M.xy -M.z]
    # Q = [s.α -conj(s.β); s.β conj(s.α)]
    # M⁺ = Q * M_mat * Q'
    # Mag(M⁺[2,1], real(M⁺[1,1]))
	Mag(
        2*conj(s.α)*s.β*M.z+conj(s.α)^2*M.xy-s.β^2*conj(M.xy),
        (abs(s.α)^2-abs(s.β)^2)*M.z-2real(s.α*s.β*conj(M.xy))#2real(s.α*s.β*conj(M.xy))=conj(s.α)*conj(s.β)*M.xy+s.α*s.β*conj(M.xy)
        )
end
#Operation on vector
# M0 = Mag(0,1)
# Mf = Ry(π/2)*M0
# Mf.xy, Mf.z


using Revise, StatisticalNonlocality, Test, LinearAlgebra
using SparseArrays

include("transition_matrix_test.jl")
include("discretization_test.jl")


@testset "utils.jl" begin
    @testset "size_function" begin
        y = ones(15)
        X = randn(15, 2)
        s = 2
        model = structural(y, s; X = X)

        n, p, m, r = size(model)
        @test n == 15
        @test p == 1
        @test m == s + size(X, 2) + 1
        @test r == 3

        y = ones(15)
        X = randn(15, 15)
        s = 2
        model = structural(y, s; X = X)
        n, p, m, r = size(model)
        @test n == 15
        @test p == 1
        @test m == s + size(X, 2) + 1
        @test r == 3
    end

    @testset "ztr" begin
        y = ones(15)
        model = local_level(y)
        Z, T, R = ztr(model)
        @test all(Z .== [1.0])
        @test T == [1.0][:, :]
        @test R == [1.0][:, :]
    end

    @testset "gram" begin
        A = [1.0][:, :]
        @test A == SSM.gram(A)
        B = [1 1.0;0 1]
        @test B*B' == SSM.gram(B)
    end

    @testset "gram_in_time" begin
        C = Array{Float64, 3}(undef, 2, 2, 5)
        B = [1 1.0;0 1]
        C[:, :, 1:5] .= B
        @test B*B' == SSM.gram_in_time(C)[:, :, 1]
        @test SSM.gram_in_time(C)[:, :, 2] == SSM.gram_in_time(C)[:, :, 1]
    end

    @testset "ensure_pos_sym!" begin
        A = randn(3, 3, 2)
        SSM.ensure_pos_sym!(A, 1)
        @test issymmetric(A[:, :, 1])
        SSM.ensure_pos_sym!(A, 2)
        @test issymmetric(A[:, :, 2])
    end

    @testset "show" begin
        y = ones(15)
        model = local_level(y)
        ss = statespace(model)
        @test show(model) == nothing
        @test show(ss) == nothing
    end

    @testset "statespace_recursion" begin
        # Unknowns error
        y = ones(15)
        model = local_level(y)
        initial_a = [1.0][:, :]
        @test_throws ErrorException("StateSpaceModel has unknown parameters.") statespace_recursion(model, initial_a)

        # initial_a dimension mismatch
        y = ones(15)
        model = local_level(y)
        model.H .= [1e-8]
        model.Q .= [1e-8]
        initial_a = [1.0 1.0]
        @test_throws ErrorException("intial_a must be a 1 by 1 matrix.") statespace_recursion(model, initial_a)

        # univariate local_level
        y = ones(15)
        model = local_level(y)
        model.H .= [1e-8]
        model.Q .= [1e-8]
        initial_a = [1.0][:, :]
        y, a = statespace_recursion(model, initial_a)
        @test y ≈ ones(15) atol = 1e-3 rtol = 1e-3
        @test a ≈ ones(15) atol = 1e-3 rtol = 1e-3

        # univariate linear_trend
        y = collect(1.0:15.0)
        model = linear_trend(y)
        model.H .= [1e-8]
        model.Q .= [1e-8 0
                    0 1e-8]
        initial_a = [1.0 0.5]
        y, a = statespace_recursion(model, initial_a)
        @test y ≈ collect(1.0:0.5:8.0) atol = 1e-3 rtol = 1e-3
        @test a ≈ [collect(1.0:0.5:8.0) 0.5*ones(15)] atol = 1e-3 rtol = 1e-3

        # multivariate local_level
        y = ones(15, 4)
        model = local_level(y)
        model.H .= Diagonal(1e-8*ones(4))
        model.Q .= Diagonal(1e-8*ones(4))
        initial_a = ones(1, 4)
        y, a = statespace_recursion(model, initial_a)
        @test y ≈ ones(15, 4) atol = 1e-3 rtol = 1e-3
        @test a ≈ ones(15, 4) atol = 1e-3 rtol = 1e-3

        # univariate linear_trend
        y_c = collect(1.0:15.0)
        y = [y_c y_c y_c y_c]
        model = linear_trend(y)
        model.H .= Diagonal(1e-8*ones(4))
        model.Q .= Diagonal(1e-8*ones(8))
        initial_a = [1.0 0.5 1.0 0.5 1.0 0.5 1.0 0.5]
        y, a = statespace_recursion(model, initial_a)
        @test y[:, 1] ≈ collect(1.0:0.5:8.0) atol = 1e-3 rtol = 1e-3
        @test y[:, 2] ≈ collect(1.0:0.5:8.0) atol = 1e-3 rtol = 1e-3
        @test y[:, 3] ≈ collect(1.0:0.5:8.0) atol = 1e-3 rtol = 1e-3
        @test y[:, 4] ≈ collect(1.0:0.5:8.0) atol = 1e-3 rtol = 1e-3
        @test a[:, [1, 2]] ≈ [collect(1.0:0.5:8.0) 0.5*ones(15)] atol = 1e-3 rtol = 1e-3
        @test a[:, [3, 4]] ≈ [collect(1.0:0.5:8.0) 0.5*ones(15)] atol = 1e-3 rtol = 1e-3
        @test a[:, [5, 6]] ≈ [collect(1.0:0.5:8.0) 0.5*ones(15)] atol = 1e-3 rtol = 1e-3
        @test a[:, [7, 8]] ≈ [collect(1.0:0.5:8.0) 0.5*ones(15)] atol = 1e-3 rtol = 1e-3
    end
end

function compare_forecast_simulation(ss::StateSpace, N::Int, S::Int, rtol::Float64)
    sim = simulate(ss, N, S)
    forec, dist = forecast(ss, N)

    @test forec ≈ mean(sim, dims = 3) rtol = rtol
end


__precompile__(true)

"""
Main module of `Expokit` -- Julia implementations of some routines contained
in [EXPOKIT](https://www.maths.uq.edu.au/expokit/).

Those routines allow an efficient calculation of the action of matrix exponentials
on vectors for large sparse matrices. For more details about the methods see
R.B. Sidje, ACM Trans. Math. Softw., 24(1):130-156, 1998 (or its
[preprint](https://www.maths.uq.edu.au/expokit/paper.pdf)).
"""
module Expokit

using LinearAlgebra

const axpy! = LinearAlgebra.axpy!
const exp! = LinearAlgebra.exp!

include("expmv.jl")
include("padm.jl")
include("chbv.jl")
include("phimv.jl")

end # module


# mpirun -n 16 julia MPI_forward.jl
using Revise
using ADSeismic
using ADCME
using JLD2
ADCME.options.customop.verbose = false
mpi_init()
M = N = Int(sqrt(mpi_size()))
n = 100

param = MPIElasticPropagatorParams(NX= n * M, NY= n * N, n=n, NSTEP=2000, DELTAT=1e-4/2,
                        DELTAX=1.0, DELTAY=1.0)
compute_PML_Params!(param)
vp = 3300.
vs = 3300. / 1.732
rho = 2800.
λ, ρ, μ = compute_default_properties(param, vp, vs, rho)
source = Ricker(param, 50.0, 200.0)

srci = [div(param.NX,5)]
srcj = [div(param.NY,2)]
srctype = [0]
srcv = repeat(source, 1, 1)
src = MPIElasticSource(param, srci, srcj, srctype, srcv)

rcvj = Array(20:n*param.M-19)
rcvi = ones(Int64, length(rcvj))*((n*param.N)÷5)
rcvtype = zeros(Int64, length(rcvj))
receiver =  MPIElasticReceiver(param, rcvi, rcvj, rcvtype)

propagator = MPIElasticPropagatorSolver(param, src, ρ, λ, μ)
MPISimulatedObservation!(propagator, receiver)

###### second source ######

srci = [div(param.NX,5)]
srcj = [div(param.NY,2)]
srctype = [0]
srcv = repeat(source, 1, 2)
src = MPIElasticSource(param, srci, srcj, srctype, srcv)

rcvj = Array(20:n*param.M-19)
rcvi = ones(Int64, length(rcvj))*((n*param.N)÷5)
rcvtype = zeros(Int64, length(rcvj))
receiver2 =  MPIElasticReceiver(param, rcvi, rcvj, rcvtype)

propagator2 = MPIElasticPropagatorSolver(param, src, ρ, λ, μ)
MPISimulatedObservation!(propagator2, receiver2)

function is_not_noop(c)
    try
        length(c)
    catch
        return false
    end
    return true
end

loss = constant(0.0)
if is_not_noop(receiver)
    loss = sum(receiver.rcvv^2) + sum(receiver2.rcvv^2)
end
loss = mpi_sum(loss)

sess = Session(); init(sess)
@info "starting..."
L = run(sess, loss)

if mpi_size()>1
    mpi_finalize()
end
# if mpi_rank()==0 && mpi_size()==1
#     close("all")
#     pcolormesh(rcvv, cmap="gray")
#     colorbar()
#     xlabel("Location")
#     ylabel("Time")
#     savefig("receiver.png")

#     close("all")
#     visualize_wavefield(Vx, param)

# end


function boundary_condition_L_elas(c)
    g = [0.0, 0.0]
    return g
end
function rhs_L_elas(c)
    f = [0.0, 0.0]
    if c[1] > 0.9
        f[1] = -1.0
    end
    return f
end
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~-------------------------
function characElt(verts)
    n, n_sides = size(verts, 1), size(verts, 1)
    area_components = verts[:, 1] .* [verts[2:end, 2]..., verts[1, 2]] .-
                     [verts[2:end, 1]..., verts[1, 1]] .* verts[:, 2]
    area = 0.5 * abs(sum(area_components))
    centroid = sum((verts .+ vcat(verts[2:end, :], verts[1, :]')) .*
                    repeat(area_components, 1, 2), dims = 1) / (6 * area)
    diameter = 0 # compute the diameter by looking at every pair of vertices
    for i = 1:(n_sides - 1)
        for j = (i + 1):n_sides
            diameter = max(diameter, norm(verts[i, :] .- verts[j, :]))
        end
    end
    return area, centroid, diameter
end
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~-------------------------
"""
   IK, JK, SK, IF, SF = assembKM_vemKsource_elas2D(pv, cells, rhs,
                                                   boundary_condition,
                                                   meshboundary)

Assemble stiffness and source terms of virtual element approach
"""
function assembKM_vemKsource_elas2D(pv, cells, rhs, boundary_condition,
                                    meshboundary)
    # material parameters
    lm = 0.5769
    mu = 0.3846
    # type of input (for overloaded differentiation calls)
    typein = typeof(pv[1])
    n_dofs, n_polys = size(pv, 1), 3 # method has 1 degree of freedom per vertex
    # stiffness coefficients and source coefficients
    IK, JK, IF = Int64[], Int64[], Int64[]
    SK, SF = if typein == Float64
        Float64[], Float64[]
    else
        Any[], Any[]
    end
    # a utility function for wrapping around a vector
    mod_wrap(x, a) = mod(x - 1, a) + 1
    # (3 * 3) matrix D of elasticity over constant strain fields
    Di = [ (2.0 * mu + lm)              lm         0.0;
                       lm   (2.0 * mu + lm)        0.0;
                      0.0              0.0    4.0 * mu]
    for el_id = 1:length(cells)
        #println(cells[el_id])
        v = cells[el_id]      # global indices of the vertices of el
        vc = pv[v, :]   # na * 2 array of vertices coordinates
        na = length(v)
        # start computing the geometric information
        vol, bar, diameter = characElt(vc)
        D = vol * Di
        # basis functions for R = [(1,0), (0,1)  , (-(y-bary, x-barx)) ]
        # basis functions for C = [(x-barx, 0), (0,y-bary) ,(y-bary, x-barx)]
        # (2n*3) Matrix WC of components of basis functions over constant
        # strain fields
        WC = zeros(typein, 2 * na, 3)
        # (2n*3) Matrix WR of components of basis functions over rigid-body
        # motions
        WR = zeros(typein, 2 * na, 3)
        # (2n*3) matrix NC: (NC)_{2i - 1,l} = (c_k(x_i))_0, (NC)_{2i,l} =
        # (c_k(x_i))_1
        NC = zeros(typein, 2 * na, 3)
        # (2n*3) matrix NR: (NR)_{2i - 1,l} = (r_k(x_i))_0, (NR)_{2i,l} =
        # (r_k(x_i))_1
        NR = zeros(typein, 2 * na, 3)
        for i = 1:na
            # coordinates of vertices i-1, i, i+1
            ci = vc[i, :] # this vertex and its neighbours
            cim = vc[mod_wrap(i - 1, na), :]
            cip = vc[mod_wrap(i + 1, na), :]
            # normal vector at vertex (multiplied by its length)
            nor = [cip[2] - cim[2], - (cip[1] - cim[1])]

            q1 = 0.5 * nor[1] / vol
            q2 = 0.5 * nor[2] / vol

            WC[2 * i - 1, 1]   = q1
            WC[2 * i - 1, 2]   = 0.0
            WC[2 * i - 1, 3]   = 0.5 * q2

            WC[2 * i, 1] = 0.0
            WC[2 * i, 2] = q2
            WC[2 * i, 3] = 0.5 * q1

            WR[2 * i - 1, 1]   = 1.0 / na
            WR[2 * i - 1, 2]   = 0.0
            WR[2 * i - 1, 3]   = 0.5 * q2

            WR[2 * i, 1] = 0.0
            WR[2 * i, 2] = 1.0 / na
            WR[2 * i, 3] = -0.5 * q1

            NC[2 * i - 1, 1] = ci[1] - bar[1]
            NC[2 * i - 1, 2] = 0.0
            NC[2 * i - 1, 3] = ci[2] - bar[2]

            NC[2 * i, 1] = 0.0
            NC[2 * i, 2] = ci[2] - bar[2]
            NC[2 * i, 3] = ci[1] - bar[1]

            NR[2 * i - 1, 1] = 1.0
            NR[2 * i - 1, 2] = 0.0
            NR[2 * i - 1, 3] = ci[2] - bar[2]

            NR[2 * i, 1] = 0.0
            NR[2 * i, 2] = 1.0
            NR[2 * i, 3] = - (ci[1] - bar[1])
        end
        # projection matrix over polynomial displacements
        PP = WC * NC' + WR * NR'
        # projection matrix over higher-order functions
        QP = eye(2 * na) - PP
        # polynomial block in the local stiffness matrix
        PROJ = WC * D * WC'
        # higher-order functions block in the local stiffness matrix
        alphaE = tr(D) / tr(NC' * NC)
        STAB = alphaE * QP * QP'
        # local stiffness matrix
        Ke = PROJ + STAB
        # copy local to global
        # K[vert_ids, vert_ids] += local_stiffness
        # F[vert_ids] .+= rhs(centroid) * area / n_sides
        for il = 1:na
            ig = v[il]
            for jl = 1:na
                jg = v[jl]
                push!(IK, 2 * ig - 1)
                push!(IK, 2 * ig - 1)
                push!(IK, 2 * ig)
                push!(IK, 2 * ig)

                push!(JK, 2 * jg - 1)
                push!(JK, 2 * jg)
                push!(JK, 2 * jg - 1)
                push!(JK, 2 * jg)

                push!(SK, Ke[2 * il - 1, 2 * jl - 1])
                push!(SK, Ke[2 * il - 1, 2 * jl])
                push!(SK, Ke[2 * il, 2 * jl - 1])
                push!(SK, Ke[2 * il, 2 * jl])
            end
        end
        # update force vector
        f = rhs(bar)
        for il = 1:na
            ig = v[il]
            push!(IF, 2 * ig - 1)
            push!(IF, 2 * ig)
            push!(SF, vol / na * f[1])
            push!(SF, vol / na * f[2])
        end
    end
    # add boundary contribution in the source term
    nb = length(meshboundary)
    boundary_vals = [boundary_condition(pv[meshboundary[k], :]) for k = 1:nb]
    # boundary dofs
    Vmeshboundary = vcat(2 * meshboundary .- 1, 2 * meshboundary)
    Jinmesh = indexin(JK, Vmeshboundary)
    Jboundary = findall(Jinmesh .!= nothing)
    for l ∈ Jboundary
        push!(IF, IK[l])
        if mod(Jinmesh[l], 2) == 1
            push!(SF, - SK[l] * boundary_vals[(Jinmesh[l] + 1) ÷ 2][1])
        else
            push!(SF, - SK[l] * boundary_vals[Jinmesh[l] ÷ 2][2])
        end
    end
    return IK, JK, SK, IF, SF
end
#-------------------------------------------------------------------------------
"""
    u, p, t, meshboundary = vem_elas2D(filename::String =
                                                         "squarepolmesh_coarse",
                                       nc::Int64 = 1_00;
                                       resolution::Int64 = 400)

N.B.
* nc = 100, 1_000 or 10_000
* adapted code from the article / matlab's code  "The virtual element method in 50 lines of matlab". See https://arxiv.org/pdf/1604.06021.pdf
"""
function vem_elas2D(filename::String = "Lpolmesh", nc::Int64 = 1_00;
                    resolution::Int64 = 400, visible::Bool = false)
    # computes the virtual element solution of the Poisson problem on
    # the specified mesh
    # load the mesh  + pv       : vertices of the cells
    #                + cells    : polygonal cells (⚠ orientation is crucial)
    #                + (pb, tb) : restricted delaunay mesh
    mesh_filename = "$(@__DIR__)/../test/data/$(filename)_$(nc).jld2"
    println("read file $(mesh_filename)")
    JLD2.@load(mesh_filename, ps, Iv, pv, bbelem, elem, pb, tb)
    cells, bcells = bbelem, bbelem
    # boundary points / rhs
    rhs, boundary_condition, meshboundary = if occursin("square", mesh_filename)
        rhs_sqr_elas, boundary_condition_sqr_elas, []
    else
        EPS = 1e-4
        rhs_L_elas, boundary_condition_L_elas, findall(pv[:, 2] .> (1 - EPS))
    end
    # call assemble function
    IK, JK, SK, IF, SF = assembKM_vemKsource_elas2D(pv, cells, rhs,
                                                    boundary_condition,
                                                    meshboundary)
    K, F, internal_dofs, u = solve_elas2D(IK, JK, SK, IF, SF, pv, meshboundary,
                                          boundary_condition)
    # plot
    if resolution > 0
        Iv = visible ? Iv : 1:length(cells)
        plotsolution(u, pv, cells, bcells, Iv, ps,
                     mesh_filename = mesh_filename, resolution = resolution)
    end
    return nothing
end
#-------------------------------------------------------------------------------
function solve_elas2D(IK, JK, SK, IF, SF, pv, meshboundary, boundary_condition)
    n_dofs = 2 * size(pv, 1)
    u = zeros(n_dofs) # degrees of freedom of the virtual element solution
    K = sparse(IK, JK, SK, n_dofs, n_dofs)
    F = Vector(sparsevec(IF, SF, n_dofs))
    nb = length(meshboundary)
    boundary_vals = [boundary_condition(pv[meshboundary[k], :]) for k = 1:nb]
    Vboundary_vals = [boundary_vals[k][l] for l = 1:2, k = 1:nb][:]
    # vertices which aren’t on the boundary
    Vmeshboundary = vcat(2 * meshboundary .- 1, 2 * meshboundary)
    internal_dofs =  setdiff(1:n_dofs, Vmeshboundary)
    # solve
    u[internal_dofs] = K[internal_dofs, internal_dofs] \ F[internal_dofs]
    u[Vmeshboundary] .= Vboundary_vals # set the boundary values
    return K, F, internal_dofs, u
end


@testset "inplace keyword" begin
    rosenbrock = MultivariateProblems.UnconstrainedProblems.examples["Rosenbrock"]
    f = MVP.objective(rosenbrock)
    g! = MVP.gradient(rosenbrock)
    h! = MVP.hessian(rosenbrock)
    function g(x)
        G = similar(x)
        g!(G, x)
        G
    end
    function h(x)
        n = length(x)
        H = similar(x, n, n)
        h!(H, x)
        H
    end
    initial_x = rosenbrock.initial_x

    inp_res = optimize(f, g, h, initial_x; inplace = false)
    op_res  = optimize(f, g!, h!, initial_x; inplace = true)

    for op in (Optim.minimizer, Optim.minimum, Optim.f_calls,
                Optim.g_calls, Optim.h_calls, Optim.iterations, Optim.converged)
        @test all(op(inp_res) .=== op(op_res))
    end
end