text:
module mmLE11NAFEMSQ8algo2
using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsDeforLinear.AlgoDeforLinearModule: linearstatics, exportdeformation,
exportstress, exportstresselementwise
using Test
import LinearAlgebra: norm, cholesky, cross
function test()
    # NAFEMS LE11 benchmark with Q8 elements.
    # # This is a test recommended by the National Agency for Finite Element
    # # Methods and Standards (U.K.): Test LE11 from NAFEMS Publication TNSB,
    # # Rev. 3, “The Standard NAFEMS Benchmarks,” October 1990.
    # #
    # # Target solution: Direct stress,   =  –105 MPa at point A.
    #function  LE11NAFEMS()
    # Parameters:
    Ea =  210000*phun("MEGA*Pa")
    nua =  0.3;
    alphaa = 2.3e-4;              # thermal expansion coefficient
    sigmaA = -105*phun("MEGA*Pa")
    nref =  1;                        # how many times should we refine the mesh?
    X = [1.     0.;#A
    1.4    0.;#B
    0.995184726672197   0.098017140329561;
    1.393258617341076 0.137223996461385;
    0.980785  0.195090;#
    1.37309939 0.27312645;
    0.956940335732209   0.290284677254462
    1.339716470025092 0.406398548156247
    0.9238795  0.38268;#C
    1.2124  0.7;#D
    0.7071  0.7071;#E
    1.1062  1.045;#F
    0.7071  (0.7071+1.79)/2;#(E+H)/2
    1.      1.39;#G
    0.7071  1.79;#H
    1.      1.79;#I
    ]*phun("M")
    tolerance  = 1.e-6*phun("M")
    ##
    # Note that the material object needs to be created with the proper
    # model-dimension reduction in mind.  In this case that is the axial symmetry
    # assumption.
    MR  =  DeforModelRed2DAxisymm

    fens = FENodeSet(X);
    fes = FESetQ4([1 2 4 3; 3 4 6 5; 5 6 8 7; 7 8 10 9; 9 10 12 11; 11 12 14 13; 13 14 16 15]);
    for ref = 1:nref
        fens,fes = Q4refine(fens,fes);
        list = selectnode(fens,distance = 1.0+0.1/2^nref, from = [0. 0.], inflate = tolerance);
        fens.xyz[list,:] =  FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);
    end
    fens,fes = Q4toQ8(fens,fes)
    list = selectnode(fens,distance = 1.0+0.1/2^nref, from = [0. 0.], inflate = tolerance);
    fens.xyz[list,:] =  FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);

    # EBC's
    l1 = selectnode(fens,box = [-Inf Inf 0 0],inflate = tolerance)
    e1 = FDataDict("node_list"=>l1, "component"=>2, "displacement"=>x -> 0.0)
    l1 = selectnode(fens,box = [-Inf Inf 1.79  1.79],inflate = tolerance)
    e2 = FDataDict("node_list"=>l1, "component"=>2, "displacement"=>x -> 0.0)

    # Temperature field
    dtemp = FDataDict("temperature"=>x -> x[1] + x[2])

    # Property and material
    material = MatDeforElastIso(MR, 0.0, Ea, nua, alphaa)

    femm  =  FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(2, 3), true), material)

    # Make region 1
    region = FDataDict("femm"=>femm)
    # Make model data
    modeldata =  FDataDict(
    "fens"=> fens, "regions"=>  [region],
    "essential_bcs"=>[e1, e2], "temperature_change"=>dtemp)

    # Call the solver
    modeldata = linearstatics(modeldata)
    geom = modeldata["geom"]
    u = modeldata["u"]
    dT = modeldata["temp"]

    modeldata["postprocessing"] = FDataDict("boundary_only"=> true,
    "file"=>"LE11NAFEMS_Q8_deformation.vtk")
    modeldata = exportdeformation(modeldata)
    # @async run(`"paraview.exe" $(modeldata["postprocessing"]["exported"][1]["file"])`)
    try rm(modeldata["postprocessing"]["exported"][1]["file"]) catch end

    nA  = selectnode(fens,box = FFlt[1.0  1.0 0.0 0.0], inflate = tolerance);

    modeldata["postprocessing"] = FDataDict("boundary_only"=> true,
    "file"=>"LE11NAFEMS_Q8_sigmay.vtk", "quantity"=>:Cauchy,
    "component"=>2)
    modeldata = exportstress(modeldata)
    modeldata["postprocessing"] = FDataDict("boundary_only"=> false,
    "file"=>"LE11NAFEMS_Q8_sigmay.vtk", "quantity"=>:Cauchy,
    "component"=>2)
    modeldata = exportstress(modeldata)
    # @async run(`"paraview.exe" $(modeldata["postprocessing"]["exported"][1]["file"])`)
    try rm(modeldata["postprocessing"]["exported"][1]["file"]) catch end
    fld =  modeldata["postprocessing"]["exported"][1]["field"]

    sA  =  fld.values[nA]/phun("MEGA*Pa")
    sAn  =  fld.values[nA]/sigmaA
    # println("Stress at point A: $(sA) i. e.  $( sAn*100  )% of reference value")
    @test norm(sA - [-93.8569]) < 1.0e-2

    # Loop over only those elements that share the node nA
    fen2fe  = FENodeToFEMap(connasarray(fes), nnodes(geom))
    function inspector(idat, elnum, conn, xe,  out,  xq)
        # println("loc = $(  xq  ) : $(  transpose(out)/phun("MEGA*Pa")  )")
        return idat
    end

    inspectintegpoints(femm, geom, u, dT,  fen2fe.map[nA[1]],
    inspector, []; quantity = :Cauchy)

    modeldata["postprocessing"] = FDataDict("boundary_only"=> false,
    "file"=>"LE11NAFEMS_Q8_sigmay_ew.vtk", "quantity"=>:Cauchy,
    "component"=>2)
    modeldata = exportstresselementwise(modeldata)
    # @async run(`"paraview.exe" $(modeldata["postprocessing"]["exported"][1]["file"])`)
    try rm(modeldata["postprocessing"]["exported"][1]["file"]) catch end
end
end
using .mmLE11NAFEMSQ8algo2
mmLE11NAFEMSQ8algo2.test()

module sscratch_06112017
using FinEtools
using FinEtoolsDeforLinear
using Test
import Statistics: mean
function test()
  ## Solid cylinder/taper/sphere—-temperature loading; quadratic brick mesh
  #

  ## Description
  #
  # The solid cylinder/taper/sphere axially-symmetric part represented in
  # Figure 1 is exposed to linearly varying temperature in the plane of the
  # cross-section. The temperature in the coordinates $r$  (the coordinate)
  # and $z$ (the axial ccoordinate)  is given as $T=r+z$. The goal is to find
  # the mechanical stress at the point A induced by the thermal expansion.
  #

  ##
  # The part is constrained against axial expansion along the faces of HIH'I'
  # and ABA'B'. The Young's modulus is 210 GPa, the Poisson's ratio is .3,
  # and the coefficient of thermal expansion is 2.3e-4/degree Celsius.

  ##
  # This is a test recommended by the National Agency for Finite Element
  # Methods and Standards (U.K.): Test LE11 from NAFEMS Publication TNSB,
  # Rev. 3, “The Standard NAFEMS Benchmarks,” October 1990.
  #
  # Target solution: Compressive  axial stress $\sigma_z$  = –105 MPa along
  # the circle passing through point A.

  ##
  # The toolkit has a helpful physical-units facility.  The function phun()
  # allows use of basic  units and basic
  # multipliers (for instance, mega).

  ##
  # Set the material properties.
  Ea = 210000*phun("MEGA*PA");# Young's modulus
  nua = 0.3;# Poisson ratio
  alphaa = 2.3e-4;# coefficient of thermal expansion

  ##
  # This is the target stress value.
  sigmaA = -105*phun("MEGA*PA");

  ##
  # The mesh  will be created in a very coarse representation from the
  # key points in the drawing. The first coordinate is radial, the second coordinate is axial.
  rz=[1.     0.;#A
  1.4    0.;#B
  0.995184726672197   0.098017140329561;
  1.393258617341076 0.137223996461385;
  0.980785  0.195090;#
  1.37309939 0.27312645;
  0.956940335732209   0.290284677254462
  1.339716470025092 0.406398548156247
  0.9238795  0.38268;#C
  1.2124  0.7;#D
  0.7071  0.7071;#E
  1.1062  1.045;#F
  0.7071  (0.7071+1.79)/2;#(E+H)/2
  1.      1.39;#G
  0.7071  1.79;#H
  1.      1.79;#I
  ]*phun("M")
  tolerance =1.e-6*phun("M")

  ##
  # Note that the material object needs to be created with the proper
  # model-dimension reduction in mind.  In this case that is the fully three-dimensional solid.
  MR = DeforModelRed3D

  # This is the quadrilateral mesh of the cross-section.   It will be modified and
  # refined as  we go.
  fens = FENodeSet(rz);
  fes = FESetQ4([1 2 4 3; 3 4 6 5; 5 6 8 7; 7 8 10 9; 9 10 12 11; 11 12 14 13; 13 14 16 15]);

  ##
  # If needed, the initial mesh  can be refined by bisection.  Just set
  # `nref` greater than zero.  Note that  the nodes located along the
  # edges are moved onto the  spherical surface when they _should be_ on
  # the spherical surface.  This is important in order to ensure
  # convergence to the proper value of the stress.  Just refining  the
  # initial mesh without repositioning of the nodes onto the spherical surface would mean that the
  # refinement would preserve a concave corner where in reality there is
  # none.  The stress would be artificially raised and convergence would
  # not be guaranteed.

  nref = 0;
  for ref = 1:nref
    fens,fes = Q4refine(fens,fes);
    list = selectnode(fens, distance=1.0+0.1/2^nref, from=[0. 0.], inflate=tolerance);
    fens.xyz[list,:] = FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);
  end

  ##
  # The mesh is extruded by sweeping around the axis of symmetry.
  # Only a single layer of elements is generated of internal angle
  # |angslice|.
  nLayers = 7;
  angslice  = 5*pi/16;

  ##
  # First the mesh is extruded to a block whose third dimension
  # represents the angular coordinate.
  fens,fes = H8extrudeQ4(fens, fes, nLayers,
  (rz,k)->[rz[1],rz[2],0.0]-(k)/nLayers*[0.,0.,angslice]);

  ##
  # The mesh is now converted to the serendipity 20-node elements.
  # We will reposition the nodes later.
  fens,fes = H8toH20(fens,fes);

  ##
  # The boundary of the block is extracted and the faces of the mesh on
  # the bounding cross-sections are identified. Recall that this is just
  # about the topology (connectivity), the geometry does not matter at
  # this point.
  bfes = meshboundary(fes);
  f1l = selectelem(fens, bfes, box=[-Inf,Inf,-Inf,Inf,0.0,0.0], inflate=tolerance);
  f2l = selectelem(fens, bfes, box=[-Inf,Inf,-Inf,Inf,-angslice,-angslice],
  inflate=tolerance);

  ##
  # The block is now converted  to the axially symmetric geometry by using the
  # third (angular) coordinate  to sweep out  an axially symmetric domain. The
  # ccoordinates of the nodes at this point are |rza|,  radial distance,
  # Z-coordinate, angle.
  sweep(rza) = [-rza[1]*sin(rza[3]+angslice/2.0), rza[1]*cos(rza[3]+angslice/2.0), rza[2]]
  for j=1:size(fens.xyz,1)
    fens.xyz[j,:] = sweep(fens.xyz[j,:])
  end

  ##
  # The nodes within the radial distance of 1.0 of the origin (i. e.
  # those on the spherical surface)  are repositioned one more time to be
  # located on the spherical surface for sure. (Recall  that we have
  # inserted additional nodes at the midpoints of the edges when the mesh
  # was converted to quadratic elements.)
  list = selectnode(fens,distance=1.0+0.1/2^nref,
  from=[0. 0. 0.], inflate=tolerance);
  fens.xyz[list,:]= FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:], 1.0);

  ##
  # We are ready to create the  finite element model machine and to use
  # it to construct  the global system for the displacements.
  ##
  # The material is created from the property object.  Note that the
  # |alpha| attribute is the thermal expansion coefficient.

  # Create isotropic elastic material
  material = MatDeforElastIso(MR, 1.0, Ea, nua, alphaa)

  ##
  # The finite element  model machine puts together the material, the
  # finite elements,  and the integration rule. The Gauss quadrature with
  # 3x3x3 points  gives good accuracy in this case. Compare it with 2x2x2
  # quadrature to appreciate the difference.

  femm = FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(3, 3)), material)

  ##
  # The geometry nodal field is created from the node set.   The
  # displacement field is created by cloning the geometry and then
  # zeroing out the nodal parameters.
  geom = NodalField(fens.xyz)
  u = NodalField(zeros(size(fens.xyz,1),3)) # displacement field
  nnodes(geom)
  ##
  # The EBCs are applied  next.  Only the axial (Z) degrees of freedom at
  # the bottom and top are fixed to zero.
  l1 = selectnode(fens, box=[-Inf Inf -Inf Inf 0.0 0.0], inflate=tolerance)
  setebc!(u, l1, true, 3, zeros(size(l1)))
  l1 = selectnode(fens, box=[-Inf Inf -Inf Inf 1.79  1.79], inflate=tolerance)
  setebc!(u, l1, true, 3, zeros(size(l1)))
  applyebc!(u)
  numberdofs!(u)

  ##
  # The restraints of the nodes on the bounding cross-sections in the direction
  # of the normal to the plane of the cross-section  in the
  # circumferential direction are introduced using a penalty formulation.
  # For that purpose we introduce  a finite element model machine for the
  # surface  finite elements on the cross-sections.
  springcoefficient =1.0 / ((abs(sigmaA)/1.0e12)/Ea)
  fl = vcat(f1l, f2l)
  xsfemm = FEMMDeforWinkler(IntegDomain(subset(bfes,fl), GaussRule(2, 3)))

  ##
  # We create the temperature field using the formula $T=r+z$.
  dT = NodalField(reshape(sqrt.(fens.xyz[:,1].^2+fens.xyz[:,2].^2)+fens.xyz[:,3],size(fens.xyz,1),1));

  ##
  # And we are ready to assemble the system matrix. Both the elastic stiffness of
  # the hexahedral elements ...
  K = stiffness(femm, geom, u)
  # ...  and the elastic stiffness    of the springs on the contact surfaces of the cross-sections.
  H = surfacenormalspringstiffness(xsfemm,  geom, u, springcoefficient, SurfaceNormal(3))

  ##
  # The mechanical loads are computed from the thermal strains.
  F = thermalstrainloads(femm, geom, u, dT)

  ##
  # And  the solution for the free degrees of freedom is obtained.
  U=  (K+H)\F
  scattersysvec!(u, U[:])

  ##
  # The stress  is recovered from the stress calculated at the
  # integration points.

  fld= fieldfromintegpoints(femm, geom, u, dT, :Cauchy, 3)

  ##
  # Now that we have the nodal field  for the axial stress, we can plot
  # the axial stress painted on the deformed geometry.

  # File =  "LE11NAFEMS_H20_sigmaz.vtk"
  # vtkexportmesh(File, fens, fes;
  # scalars=[("sigmaz", fld.values)], vectors=[("u", u.values)])
  # @async run(`"paraview.exe" $File`)
  # File =  "LE11NAFEMS_H20_dT.vtk"
  # vtkexportmesh(File, fens, fes; scalars=dT.values,scalars_name ="dT", vectors=u.values,vectors_name="u")

  ##
  # The  computed stress at the node that is located at the point A  is
  # going to be now extracted from the nodal field for the stress.
  # Nodes at level Z=0.0
  l1 =selectnode(fens,box=FFlt[-Inf  Inf -Inf  Inf 0.0 0.0],inflate=tolerance);
  l2 =selectnode(fens,distance=1.0+0.1/2^nref,from=FFlt[0.0 0.0 0.0],inflate=tolerance);
  nA=intersect(l1,l2);
  sA = mean(fld.values[nA])/phun("MEGA*Pa")
  sAn = mean(fld.values[nA])/sigmaA
  # println("Stress at point A: $(sA) i. e.  $( sAn*100  )% of reference value")

  @test abs(sA-(-83.7322285847101))/(-83.7322285847101) < 1.e-3
  ## Discussion
  #
  ##
  # The 3-D solution corresponds well to the 2-D axially symmetric model.
  # We also see good correspondence to other published solutions for
  # comparable finite element models.  For instance, Abaqus 6.11
  # Benchmark manual lists very similar numbers.
end
end
using .sscratch_06112017
sscratch_06112017.test()

module cookstress_1
using Test
using FinEtools
using FinEtoolsDeforLinear
using FinEtools.MeshExportModule
import LinearAlgebra: norm, cholesky, cross
function test()
  # println("Cook membrane problem,  plane stress."        )
  t0 = time()

  E = 1.0;
  nu = 1.0/3;
  width = 48.0; height = 44.0; thickness  = 1.0;
  free_height  = 16.0;
  Mid_edge  = [48.0, 52.0];# Location of tracked  deflection
  magn = 1.0/free_height;# Magnitude of applied load
  convutip = 23.97;
  n = 32;#*int(round(sqrt(170.)/2.)); # number of elements per side
  tolerance = minimum([width, height])/n/1000.;#Geometrical tolerance

  fens, fes = T3block(width, height,  n,  n)

  # Reshape into a trapezoidal panel
  for i=1:count(fens)
    fens.xyz[i, 2]=fens.xyz[i, 2]+(fens.xyz[i, 1]/width)*(height -fens.xyz[i, 2]/height*(height-free_height));
  end

  geom = NodalField(fens.xyz)
  u = NodalField(zeros(size(fens.xyz, 1), 2)) # displacement field

  l1 = selectnode(fens; box=[0, 0, -Inf,  Inf],  inflate = tolerance)
  setebc!(u, l1, 1, 0.0)
  setebc!(u, l1, 2, 0.0)
  applyebc!(u)
  numberdofs!(u)

  boundaryfes =  meshboundary(fes);
  Toplist = selectelem(fens, boundaryfes,  box= [width,  width,  -Inf,  Inf ],  inflate=  tolerance);
  el1femm =  FEMMBase(IntegDomain(subset(boundaryfes, Toplist),  GaussRule(1, 2)))
  fi = ForceIntensity([0.0, +magn]);
  F2 = distribloads(el1femm,  geom,  u,  fi,  2);

  MR = DeforModelRed2DStress
  material = MatDeforElastIso(MR,  0.0, E, nu, 0.0)

  femm = FEMMDeforLinear(MR, IntegDomain(fes,  TriRule(1)),  material)

  K = stiffness(femm,  geom,  u)
  K = cholesky(K)
  U=  K\(F2)
  scattersysvec!(u, U[:])

  nl = selectnode(fens,  box=[Mid_edge[1], Mid_edge[1], Mid_edge[2], Mid_edge[2]], inflate=tolerance);
  theutip = zeros(FFlt, 1, 2)
  gathervalues_asmat!(u, theutip, nl);
  # println("$(time()-t0) [s];  displacement =$(theutip[2]) as compared to converged $convutip")

  # File =  "a.vtk"
  # vtkexportmesh(File,  fes.conn,  geom.values+u.values,
  # FinEtools.MeshExportModule.VTK.T3; vectors=[("u", u.values)])

  @test abs(theutip[2]-23.8155)/23.8155 < 1.e-3 # FinEALE solution
end
end
using .cookstress_1
cookstress_1.test()

module scratch1_06092017

using FinEtools
using FinEtoolsDeforLinear
using Test

mutable struct MyIData
  c::FInt
  r::FFltVec
  s::FFltVec
end

function test()

  # println("Thick pipe with internal pressure: axially symmetric model")
  #=
  This is a simple modification of the full three-dimensional simulation of
  the tutorial pub_thick_pipe that implements the axially-symmetric model
  reduction procedure.

  An infinitely long thick walled cylindrical pipe
  with inner boundary radius of 3 mm and outer boundary radius of 9 mm is
  subjected to an internal pressure of 1.0 MPa. A wedge   with thickness of
  2 mm and a 90-degree angle sector is considered for the finite element
  analysis. The material properties are taken as  isotropic linear elastic
  with $E=1000$ MPa and $\nu=0.4999$ to represent nearly incompressible
  behavior. This problem has been proposed to by MacNeal and Harder as a
  test of an element's ability to represent the  response of a nearly
  incompressible material. The plane-strain condition is assumed in the
  axial direction of the pipe which together with the radial symmetry
  confines the material in all but the radial direction and therefore
  amplifies the numerical difficulties associated with the confinement of
  the nearly incompressible material.

  There is an analytical solution to this problem. Timoshenko and Goodier
  presented the original solution of Lame in their textbook. We are going
  to compare with  both the stress distribution (radial and hoop stresses)
  and the displacement of the inner  cylindrical surface.

  References:
  - Macneal RH, Harder RL (1985) A proposed standard set of problems to test
  finite element accuracy. Finite Elements in Analysis and Design 1: 3-20.
  - Timoshenko S. and Goodier J. N., Theory of Elasticity, McGraw-Hill, 2nd ed., 1951.

  =#

  # Internal radius of the pipe.
  a=3*phun("MM");
  ##
  # External radius of the pipe.
  b = 9*phun("MM");
  ##
  # Thickness of the slice.
  t = 2*phun("MM");

  ##
  # Geometrical tolerance.
  tolerance   = a/10000.;
  ##
  # Young's modulus and Poisson's ratio.
  E = 1000*phun("MEGA*PA");
  nu = 0.499;
  ##
  # Applied pressure on the internal surface.
  press =   1.0*phun("MEGA*PA");

  ##
  # Analytical solutions.   Radial stress:
  radial_stress(r)  = press*a.^2/(b^2-a^2).*(1-(b^2)./r.^2);
  ##
  # Circumferential (hoop) stress:
  hoop_stress(r) = press*a.^2/(b^2-a^2).*(1+(b^2)./r.^2);

  ##
  # Radial displacement:
  radial_displacement(r) = press*a^2*(1+nu)*(b^2+r.^2*(1-2*nu))/(E*(b^2-a^2).*r);;

  ##
  # Therefore the radial displacement of the loaded surface will be:
  urex  =  radial_displacement(a);

  ##
  # The mesh parameters: The numbers of element edges axially,
  # and through the thickness of the pipe wall (radially).

  na = 1; nt = 10;

  ##
  # Note that the material object needs to be created with the proper
  # model-dimension reduction in effect.  In this case that is the axial symmetry
  # assumption.
  MR = DeforModelRed2DAxisymm
  axisymmetric = true

  # Create the mesh and initialize the geometry.  First we are going
  # to construct the block of elements with the first coordinate
  # corresponding to the thickness in the radial direction, and the second
  # coordinate is the thickness in the axial direction.
  fens,fes  =   Q8block(b-a, t, nt, na);

  # Extract the boundary  and mark the finite elements on the
  # interior surface.
  bdryfes = meshboundary(fes);

  bcl = selectelem(fens, bdryfes, box=[0.,0.,-Inf,Inf], inflate=tolerance);
  internal_fenids= connectednodes(subset(bdryfes,bcl));
  # Now  shape the block  into  the actual wedge piece of the pipe.
  for i=1:count(fens)
    fens.xyz[i,:] = fens.xyz[i,:] + [a; 0.0];
  end

  # now we create the geometry and displacement fields
  geom = NodalField(fens.xyz)
  u = NodalField(zeros(size(fens.xyz,1),2)) # displacement field

  # The plane-strain condition in the axial direction  is specified by selecting nodes
  # on the plane y=0 and y=t.
  l1 = selectnode(fens; box=[-Inf Inf 0.0 0.0], inflate = tolerance)
  setebc!(u, l1, true, 2, 0.0)
  l1 = selectnode(fens; box=[-Inf Inf t t], inflate = tolerance)
  setebc!(u, l1, true, 2, 0.0)

  applyebc!(u)
  numberdofs!(u)

  # The traction boundary condition is applied in the radial
  # direction.

  el1femm =  FEMMBase(IntegDomain(subset(bdryfes,bcl), GaussRule(1, 3), axisymmetric))
  fi = ForceIntensity([press; 0.0]);
  F2= distribloads(el1femm, geom, u, fi, 2);

  # Property and material
  material = MatDeforElastIso(MR,  E, nu)

  femm = FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(2, 2), axisymmetric), material)

  K = stiffness(femm, geom, u)
  #K=cholesky(K)
  U =  K\(F2)
  scattersysvec!(u,U[:])

  # Transfer the solution of the displacement to the nodes on the
  # internal cylindrical surface and convert to
  # cylindrical-coordinate displacements there.
  uv=u.values[internal_fenids,:]
  # Report the  relative displacement on the internal surface:
  # println("(Approximate/true displacement) at the internal surface: $( mean(uv[:,1])/urex*100  ) %")

  # Produce a plot of the radial stress component in the cylindrical
  # coordinate system. Note that this is the usual representation of
  # stress using nodal stress field.

  fld = fieldfromintegpoints(femm, geom, u, :Cauchy, 1)

  # File =  "thick_pipe_sigmax.vtk"
  # vtkexportmesh(File, fens, fes; scalars=[("sigmax", fld.values)])

  # Produce a plot of the solution components in the cylindrical
  # coordinate system.

  function inspector(idat::MyIData, elnum, conn, xe,  out,  xq)
    push!(idat.r, xq[1])
    push!(idat.s, out[idat.c])
    return idat
  end

  idat = MyIData(1, FInt[], FInt[])
  idat = inspectintegpoints(femm, geom, u, collect(1:count(fes)),
  inspector, idat, :Cauchy)

  # using Plots
  # plotly()
  #
  # # Plot the analytical solution.
  # r = linearspace(a,b,100);
  # plot(r, radial_stress(r))
  # # Plot the computed  integration-point data
  # plot!(idat.r, idat.s, m=:circle, color=:red)
  # gui()

  @test  abs(idat.r[1]-0.003126794919243112)<1.0e-9
  @test  abs(idat.s[1]- -910911.9777008593)<1.0e-2

  ## Discussion
  #
  ##
  # The axially symmetric model is clearly very effective
  # computationally, as the size is much reduced compared to the 3-D
  # model.  In conjunction with  uniform or selective reduced integration
  # it can be very accurate as well.
end
end
using .scratch1_06092017
scratch1_06092017.test()

module scratch2_06102017

using FinEtools
using FinEtoolsDeforLinear
using Test
import Arpack: eigs
function test()
  # println("""
  #         % Vibration modes of unit cube  of almost incompressible material.
  #         %
  #         % Reference: Puso MA, Solberg J (2006) A stabilized nodally integrated
  #         % tetrahedral. International Journal for Numerical Methods in
  #         % Engineering 67: 841-867.""")
  #         t0 = time()

  E = 1*phun("PA");
  nu = 0.499;
  rho = 1*phun("KG/M^3");
  a = 1*phun("M"); b = a; h =  a;
  n1 = 10;# How many element edges per side?
  na =  n1; nb =  n1; nh  = n1;
  neigvs = 20                   # how many eigenvalues
  OmegaShift = (0.01*2*pi)^2;

  MR = DeforModelRed3D
  fens,fes  = H20block(a,b,h, na,nb,nh)

  geom = NodalField(fens.xyz)
  u = NodalField(zeros(size(fens.xyz,1),3)) # displacement field

  numberdofs!(u)

  material=MatDeforElastIso(MR, rho, E, nu, 0.0)

  femm = FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(3,2)), material)

  K =stiffness(femm, geom, u)
  femm = FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(3,3)), material)
  M =mass(femm, geom, u)
  d,v,nev,nconv = eigs(K+OmegaShift*M, M; nev=neigvs, which=:SM, explicittransform=:none)
  d = d .- OmegaShift;
  fs = real(sqrt.(complex(d)))/(2*pi)
  # println("Eigenvalues: $fs [Hz]")

  # mode = 17
  # scattersysvec!(u, v[:,mode])
  # File =  "unit_cube_modes.vtk"
  # vtkexportmesh(File, fens, fes; vectors=[("mode$mode", u.values)])

  @test abs(fs[7]-0.26259869196259) < 1.0e-5
end
end

using .scratch2_06102017
scratch2_06102017.test()

module mxxxx1_06102017

using FinEtools
using FinEtoolsDeforLinear
using FinEtoolsDeforLinear.AlgoDeforLinearModule
import Statistics: mean
using Test

# println("""
# The initially twisted cantilever beam is one of the standard test
# problems for verifying the finite-element accuracy [1]. The beam is
# clamped at one end and loaded either with unit in-plane or
# unit out-of-plane force at the other. The centroidal axis of the beam is
# straight at the undeformed  configuration, while its cross-sections are
# twisted about the centroidal axis from 0 at the clamped end to pi/2 at
# the free end.
#
# Reference:
# Zupan D, Saje M (2004) On "A proposed standard set of problems to test
# finite element accuracy": the twisted beam. Finite Elements in Analysis
# and Design 40: 1445-1451.
# """)
function  Twisted_beam(dir)
  E = 0.29e8;
  nu = 0.22;
  W = 1.1;
  L = 12.;
  t =  0.32;
  nl = 2; nt = 1; nw = 1; ref = 7;
  p =   1/W/t;
  #  Loading in the Z direction
  if dir == 2
    loadv = [0;p;0]; dir = 2; uex = 0.001753248285256; # Harder: 1.754e-3;
  else
    loadv = [0;0;p]; dir = 3; uex = 0.005424534868469; # Harder: 5.424e-3;
  end

  # loadv = [0;0;p]; dir = 3; uex = 0.005424534868469; # Harder: 5.424e-3;
  #   Loading in the Y direction
  #loadv = [0;p;0]; dir = 2; uex = 0.001753248285256; # Harder: 1.754e-3;
  tolerance  = t/1000;

  fens,fes  = H20block(L,W,t, nl*ref,nw*ref,nt*ref)

  # Reshape into a twisted beam shape
  for i = 1:count(fens)
    a = fens.xyz[i,1]/L*(pi/2); y = fens.xyz[i,2]-(W/2); z = fens.xyz[i,3]-(t/2);
    fens.xyz[i,:] = [fens.xyz[i,1],y*cos(a)-z*sin(a),y*sin(a)+z*cos(a)];
  end

  # Clamped end of the beam
  l1  = selectnode(fens; box = [0 0 -100*W 100*W -100*W 100*W], inflate  =  tolerance)
  e1 = FDataDict("node_list"=>l1, "component"=>1, "displacement"=>0.0)
  e2 = FDataDict("node_list"=>l1, "component"=>2, "displacement"=>0.0)
  e3 = FDataDict("node_list"=>l1, "component"=>3, "displacement"=>0.0)

  # Traction on the opposite edge
  boundaryfes  =   meshboundary(fes);
  Toplist   = selectelem(fens,boundaryfes, box =  [L L -100*W 100*W -100*W 100*W], inflate =   tolerance);
  el1femm  = FEMMBase(IntegDomain(subset(boundaryfes,Toplist), GaussRule(2, 2)))
  flux1 = FDataDict("femm"=>el1femm, "traction_vector"=>loadv)

  # Make the region
  MR = DeforModelRed3D
  material = MatDeforElastIso(MR, 00.0, E, nu, 0.0)
  region1 = FDataDict("femm"=>FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(3,2)),
  material))

  # Make model data
  modeldata =  FDataDict(
  "fens"=> fens, "regions"=>  [region1],
  "essential_bcs"=>[e1, e2, e3], "traction_bcs"=>  [flux1])

  # Call the solver
  modeldata = AlgoDeforLinearModule.linearstatics(modeldata)
  geom = modeldata["geom"]
  u = modeldata["u"]

  # Extract the solution
  nl = selectnode(fens, box = [L L -100*W 100*W -100*W 100*W],inflate = tolerance);
  nl1 = selectnode(fens, nearestto = fens.xyz[nl[1],:]+ tolerance/30*rand(size(fens.xyz, 2)));
  @test nl[1] == nl1[1]
  theutip = mean(u.values[nl,:], dims = 1)
  # println("displacement  = $(theutip[dir]) as compared to converged $uex")
  @test    abs(theutip[dir]-uex)<1.0e-5

  # # Write out mesh with displacements
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam")
  # modeldata = AlgoDeforLinearModule.exportdeformation(modeldata)
  #
  # # Write out mesh with stresses
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam",
  # "quantity"=> :Cauchy, "component"=> :xy)
  # modeldata = AlgoDeforLinearModule.exportstress(modeldata)
  #
  # # Write out mesh with stresses
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam",
  # "quantity"=> :Cauchy, "component"=> :xz)
  # modeldata = AlgoDeforLinearModule.exportstress(modeldata)
  #
  # # Write out mesh with von Mises stresses
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam",
  # "quantity"=> :vm)
  # modeldata = AlgoDeforLinearModule.exportstress(modeldata)
  #
  # # Write out mesh with von Mises stresses, elementwise
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam-ew",
  # "quantity"=> :vm)
  # modeldata = AlgoDeforLinearModule.exportstresselementwise(modeldata)
  #
  # # Write out mesh with von Mises stresses, elementwise
  # modeldata["postprocessing"] = FDataDict("file"=>"twisted_beam-ew",
  # "quantity"=> :Cauchy, "component"=> :xz)
  # modeldata = AlgoDeforLinearModule.exportstresselementwise(modeldata)
  #
  # println("Done")
  true

end

function test()
  Twisted_beam(2)
  Twisted_beam(3)
end

end
using .mxxxx1_06102017
mxxxx1_06102017.test()

module mx_06112017
using FinEtools
using FinEtoolsDeforLinear
using Test
import Statistics: mean
function test()
  ## Solid cylinder/taper/sphere—-temperature loading; quadratic brick mesh
  #

  ## Description
  #
  # The solid cylinder/taper/sphere axially-symmetric part represented in
  # Figure 1 is exposed to linearly varying temperature in the plane of the
  # cross-section. The temperature in the coordinates $r$  (the coordinate)
  # and $z$ (the axial ccoordinate)  is given as $T=r+z$. The goal is to find
  # the mechanical stress at the point A induced by the thermal expansion.
  #

  ##
  # The part is constrained against axial expansion along the faces of HIH'I'
  # and ABA'B'. The Young's modulus is 210 GPa, the Poisson's ratio is .3,
  # and the coefficient of thermal expansion is 2.3e-4/degree Celsius.

  ##
  # This is a test recommended by the National Agency for Finite Element
  # Methods and Standards (U.K.): Test LE11 from NAFEMS Publication TNSB,
  # Rev. 3, “The Standard NAFEMS Benchmarks,” October 1990.
  #
  # Target solution: Compressive  axial stress $\sigma_z$  = –105 MPa along
  # the circle passing through point A.

  ##
  # The toolkit has a helpful physical-units facility.  The function phun()
  # allows use of basic  units and basic
  # multipliers (for instance, mega).

  ##
  # Set the material properties.
  Ea = 210000*phun("MEGA*PA");# Young's modulus
  nua = 0.3;# Poisson ratio
  alphaa = 2.3e-4;# coefficient of thermal expansion

  ##
  # This is the target stress value.
  sigmaA = -105*phun("MEGA*PA");

  ##
  # The mesh  will be created in a very coarse representation from the
  # key points in the drawing. The first coordinate is radial, the second coordinate is axial.
  rz=[1.     0.;#A
  1.4    0.;#B
  0.995184726672197   0.098017140329561;
  1.393258617341076 0.137223996461385;
  0.980785  0.195090;#
  1.37309939 0.27312645;
  0.956940335732209   0.290284677254462
  1.339716470025092 0.406398548156247
  0.9238795  0.38268;#C
  1.2124  0.7;#D
  0.7071  0.7071;#E
  1.1062  1.045;#F
  0.7071  (0.7071+1.79)/2;#(E+H)/2
  1.      1.39;#G
  0.7071  1.79;#H
  1.      1.79;#I
  ]*phun("M")
  tolerance =1.e-6*phun("M")

  ##
  # Note that the material object needs to be created with the proper
  # model-dimension reduction in mind.  In this case that is the fully three-dimensional solid.
  MR = DeforModelRed3D

  # This is the quadrilateral mesh of the cross-section.   It will be modified and
  # refined as  we go.
  fens = FENodeSet(rz);
  fes = FESetQ4([1 2 4 3; 3 4 6 5; 5 6 8 7; 7 8 10 9; 9 10 12 11; 11 12 14 13; 13 14 16 15]);

  ##
  # If needed, the initial mesh  can be refined by bisection.  Just set
  # `nref` greater than zero.  Note that  the nodes located along the
  # edges are moved onto the  spherical surface when they _should be_ on
  # the spherical surface.  This is important in order to ensure
  # convergence to the proper value of the stress.  Just refining  the
  # initial mesh without repositioning of the nodes onto the spherical surface would mean that the
  # refinement would preserve a concave corner where in reality there is
  # none.  The stress would be artificially raised and convergence would
  # not be guaranteed.

  nref = 0;
  for ref = 1:nref
    fens,fes = Q4refine(fens,fes);
    list = selectnode(fens, distance=1.0+0.1/2^nref, from=[0. 0.], inflate=tolerance);
    fens.xyz[list,:] = FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);
  end

  ##
  # The mesh is extruded by sweeping around the axis of symmetry.
  # Only a single layer of elements is generated of internal angle
  # |angslice|.
  nLayers = 7;
  angslice  = 5*pi/16;

  ##
  # First the mesh is extruded to a block whose third dimension
  # represents the angular coordinate.
  fens,fes = H8extrudeQ4(fens, fes, nLayers,
  (rz,k)->[rz[1],rz[2],0.0]-(k)/nLayers*[0.,0.,angslice]);

  ##
  # The mesh is now converted to the serendipity 20-node elements.
  # We will reposition the nodes later.
  fens,fes = H8toH20(fens,fes);

  ##
  # The boundary of the block is extracted and the faces of the mesh on
  # the bounding cross-sections are identified. Recall that this is just
  # about the topology (connectivity), the geometry does not matter at
  # this point.
  bfes = meshboundary(fes);
  f1l = selectelem(fens, bfes, box=[-Inf,Inf,-Inf,Inf,0.0,0.0], inflate=tolerance);
  f2l = selectelem(fens, bfes, box=[-Inf,Inf,-Inf,Inf,-angslice,-angslice],
  inflate=tolerance);

  ##
  # The block is now converted  to the axially symmetric geometry by using the
  # third (angular) coordinate  to sweep out  an axially symmetric domain. The
  # ccoordinates of the nodes at this point are |rza|,  radial distance,
  # Z-coordinate, angle.
  sweep(rza) = [-rza[1]*sin(rza[3]+angslice/2.0), rza[1]*cos(rza[3]+angslice/2.0), rza[2]]
  for j=1:size(fens.xyz,1)
    fens.xyz[j,:] = sweep(fens.xyz[j,:])
  end

  ##
  # The nodes within the radial distance of 1.0 of the origin (i. e.
  # those on the spherical surface)  are repositioned one more time to be
  # located on the spherical surface for sure. (Recall  that we have
  # inserted additional nodes at the midpoints of the edges when the mesh
  # was converted to quadratic elements.)
  list = selectnode(fens,distance=1.0+0.1/2^nref,
  from=[0. 0. 0.], inflate=tolerance);
  fens.xyz[list,:]= FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:], 1.0);

  ##
  # We are ready to create the  finite element model machine and to use
  # it to construct  the global system for the displacements.
  ##
  # The material is created from the property object.  Note that the
  # |alpha| attribute is the thermal expansion coefficient.

  # Create isotropic elastic material
  material = MatDeforElastIso(MR, 1.0, Ea, nua, alphaa)

  ##
  # The finite element  model machine puts together the material, the
  # finite elements,  and the integration rule. The Gauss quadrature with
  # 3x3x3 points  gives good accuracy in this case. Compare it with 2x2x2
  # quadrature to appreciate the difference.

  femm = FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(3, 3)), material)

  ##
  # The geometry nodal field is created from the node set.   The
  # displacement field is created by cloning the geometry and then
  # zeroing out the nodal parameters.
  geom = NodalField(fens.xyz)
  u = NodalField(zeros(size(fens.xyz,1),3)) # displacement field
  nnodes(geom)
  ##
  # The EBCs are applied  next.  Only the axial (Z) degrees of freedom at
  # the bottom and top are fixed to zero.
  l1 = selectnode(fens, box=[-Inf Inf -Inf Inf 0.0 0.0], inflate=tolerance)
  setebc!(u, l1, true, 3, zeros(size(l1)))
  l1 = selectnode(fens, box=[-Inf Inf -Inf Inf 1.79  1.79], inflate=tolerance)
  setebc!(u, l1, true, 3, zeros(size(l1)))
  applyebc!(u)
  numberdofs!(u)

  ##
  # The restraints of the nodes on the bounding cross-sections in the direction
  # of the normal to the plane of the cross-section  in the
  # circumferential direction are introduced using a penalty formulation.
  # For that purpose we introduce  a finite element model machine for the
  # surface  finite elements on the cross-sections.
  springcoefficient =1.0 / ((abs(sigmaA)/1.0e12)/Ea)
  fl = vcat(f1l, f2l)
  xsfemm = FEMMDeforWinkler(IntegDomain(subset(bfes,fl), GaussRule(2, 3)))

  ##
  # We create the temperature field using the formula $T=r+z$.
  dT = NodalField(reshape(sqrt.(fens.xyz[:,1].^2+fens.xyz[:,2].^2)+fens.xyz[:,3],size(fens.xyz,1),1));

  ##
  # And we are ready to assemble the system matrix. Both the elastic stiffness of
  # the hexahedral elements ...
  K = stiffness(femm, geom, u)
  # ...  and the elastic stiffness    of the springs on the contact surfaces of the cross-sections.
  H = surfacenormalspringstiffness(xsfemm,  geom, u, springcoefficient, SurfaceNormal(3))

  ##
  # The mechanical loads are computed from the thermal strains.
  F = thermalstrainloads(femm, geom, u, dT)

  ##
  # And  the solution for the free degrees of freedom is obtained.
  U=  (K+H)\F
  scattersysvec!(u, U[:])

  ##
  # The stress  is recovered from the stress calculated at the
  # integration points.

  fld= fieldfromintegpoints(femm, geom, u, dT, :Cauchy, 3)

  ##
  # Now that we have the nodal field  for the axial stress, we can plot
  # the axial stress painted on the deformed geometry.

  # File =  "LE11NAFEMS_H20_sigmaz.vtk"
  # vtkexportmesh(File, fens, fes;
  #      scalars=[("sigmaz", fld.values)], vectors=[("u", u.values)])
  # @async run(`"paraview.exe" $File`)
  # File =  "LE11NAFEMS_H20_dT.vtk"
  # vtkexportmesh(File, fens, fes; scalars=dT.values,scalars_name ="dT", vectors=u.values,vectors_name="u")

  ##
  # The  computed stress at the node that is located at the point A  is
  # going to be now extracted from the nodal field for the stress.
  # Nodes at level Z=0.0
  l1 =selectnode(fens,box=FFlt[-Inf  Inf -Inf  Inf 0.0 0.0],inflate=tolerance);
  l2 =selectnode(fens,distance=1.0+0.1/2^nref,from=FFlt[0.0 0.0 0.0],inflate=tolerance);
  nA=intersect(l1,l2);
  sA = mean(fld.values[nA])/phun("MEGA*Pa")
  sAn = mean(fld.values[nA])/sigmaA
  # println("Stress at point A: $(sA) i. e.  $( sAn*100  )% of reference value")

  @test abs(sA-(-83.7322285847101)) < 1e-4

  ## Discussion
  #
  ##
  # The 3-D solution corresponds well to the 2-D axially symmetric model.
  # We also see good correspondence to other published solutions for
  # comparable finite element models.  For instance, Abaqus 6.11
  # Benchmark manual lists very similar numbers.
end

end
using .mx_06112017
mx_06112017.test()

module my_06112017

using FinEtools
using FinEtoolsDeforLinear
using Test
function test()
  # NAFEMS LE11 benchmark with Q8 elements.
  # # This is a test recommended by the National Agency for Finite Element
  # # Methods and Standards (U.K.): Test LE11 from NAFEMS Publication TNSB,
  # # Rev. 3, “The Standard NAFEMS Benchmarks,” October 1990.
  # #
  # # Target solution: Direct stress,   =  –105 MPa at point A.
  #function  LE11NAFEMS()
  # Parameters:
  Ea =  210000*phun("MEGA*Pa")
  nua =  0.3;
  alphaa = 2.3e-4;              # thermal expansion coefficient
  sigmaA = -105*phun("MEGA*Pa")
  nref =  1;                        # how many times should we refine the mesh?
  X = [1.     0.;#A
  1.4    0.;#B
  0.995184726672197   0.098017140329561;
  1.393258617341076 0.137223996461385;
  0.980785  0.195090;#
  1.37309939 0.27312645;
  0.956940335732209   0.290284677254462
  1.339716470025092 0.406398548156247
  0.9238795  0.38268;#C
  1.2124  0.7;#D
  0.7071  0.7071;#E
  1.1062  1.045;#F
  0.7071  (0.7071+1.79)/2;#(E+H)/2
  1.      1.39;#G
  0.7071  1.79;#H
  1.      1.79;#I
  ]*phun("M")
  tolerance  = 1.e-6*phun("M")
  ##
  # Note that the material object needs to be created with the proper
  # model-dimension reduction in mind.  In this case that is the axial symmetry
  # assumption.
  MR  =  DeforModelRed2DAxisymm

  fens = FENodeSet(X);
  fes = FESetQ4([1 2 4 3; 3 4 6 5; 5 6 8 7; 7 8 10 9; 9 10 12 11; 11 12 14 13; 13 14 16 15]);
  for ref = 1:nref
    fens,fes = Q4refine(fens,fes);
    list = selectnode(fens,distance = 1.0+0.1/2^nref, from = [0. 0.], inflate = tolerance);
    fens.xyz[list,:] =  FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);
  end
  fens,fes = Q4toQ8(fens,fes)
  list = selectnode(fens,distance = 1.0+0.1/2^nref, from = [0. 0.], inflate = tolerance);
  fens.xyz[list,:] =  FinEtools.MeshUtilModule.ontosphere(fens.xyz[list,:],1.0);

  #     File  =   "mesh.vtk"
  # vtkexportmesh(File, fens, fes)

  # now we create the geometry and displacement fields
  geom  =  NodalField(fens.xyz)
  u  =  NodalField(zeros(size(fens.xyz,1),2)) # displacement field

  # Apply EBC's
  l1 = selectnode(fens,box = [-Inf Inf 0 0],inflate = tolerance)
  setebc!(u, l1, true, 2, 00.0)
  l1 = selectnode(fens,box = [-Inf Inf 1.79  1.79],inflate = tolerance)
  setebc!(u, l1, true, 2, 00.0)
  applyebc!(u)
  numberdofs!(u)

  # Temperature field
  dT  = NodalField(reshape(fens.xyz[:,1]+fens.xyz[:,2],size(fens.xyz,1),1));

  # Property and material
  material = MatDeforElastIso(MR, 0.0, Ea, nua, alphaa)

  femm  =  FEMMDeforLinear(MR, IntegDomain(fes, GaussRule(2, 3), true), material)

  K  = stiffness(femm, geom, u)
  F  =  thermalstrainloads(femm, geom, u, dT)
  #K = cholesky(K)
  U =   K\F
  scattersysvec!(u, U[:])

  nA  = selectnode(fens,box = FFlt[1.0  1.0 0.0 0.0], inflate = tolerance);

  fld =  fieldfromintegpoints(femm, geom, u, dT, :Cauchy, 2)

  File  =   "LE11NAFEMS_Q8_sigmay.vtk"
  vtkexportmesh(File, fens, fes; scalars = [("sigmay", fld.values)],
  vectors = [("u", u.values)])
  try rm(File); catch end

  sA  =  fld.values[nA]/phun("MEGA*Pa")
  sAn  =  fld.values[nA]/sigmaA
  # println("Stress at point A: $(sA) i. e.  $( sAn*100  )% of reference value")
  @test abs(sA[1]-(-93.8569)) < 1e-3

  fen2fe  = FENodeToFEMap(connasarray(fes), nnodes(geom))
  function inspector(idat, elnum, conn, xe,  out,  xq)
    # println("loc = $(  xq  ) : $(  transpose(out)/phun("MEGA*Pa")  )")
    return idat
  end

  inspectintegpoints(femm, geom, u, dT,  fen2fe.map[nA[1]],
  inspector, []; quantity = :Cauchy)

  #finealemesh(fens,fes,"meshmfile")

  # end
  # LE11NAFEMS()
end

end
using .my_06112017
my_06112017.test()


text:
# Expression-generator for vmap!
function vmap_quote(N, ::Type{T}) where {T}
    W, Wshift = VectorizationBase.pick_vector_width_shift(T)
    val = Expr(:call, Expr(:curly, :Val, W))
    q = Expr(:block, Expr(:(=), :M, Expr(:call, :length, :dest)), Expr(:(=), :vdest, Expr(:call, :pointer, :dest)), Expr(:(=), :m, 0))
    fcall = Expr(:call, :f)
    loopbody = Expr(:block, Expr(:call, :vstore!, Expr(:call, :gep, :vdest, :m), fcall), Expr(:(+=), :m, W))
    fcallmask = Expr(:call, :f)
    bodymask = Expr(:block, Expr(:(=), :__mask__, Expr(:call, :mask, val, Expr(:call, :&, :M, W-1))), Expr(:call, :vstore!, Expr(:call, :gep, :vdest, :m), fcallmask, :__mask__))
    for n ∈ 1:N
        arg_n = Symbol(:varg_,n)
        push!(q.args, Expr(:(=), arg_n, Expr(:macrocall, Symbol("@inbounds"), LineNumberNode(@__LINE__,Symbol(@__FILE__)), Expr(:call, :pointer, Expr(:ref, :args, n)))))
        push!(fcall.args, Expr(:call, :vload, val, Expr(:call, :gep, arg_n, :m)))
        push!(fcallmask.args, Expr(:call, :vload, val, Expr(:call, :gep, arg_n, :m), :__mask__))
    end
    loop = Expr(:for, Expr(:(=), :_, Expr(:call, :(:), 0, Expr(:call, :-, Expr(:call, :(>>>), :M, Wshift), 1))), loopbody)
    push!(q.args, loop)
    ifmask = Expr(:if, Expr(:call, :(!=), :m, :M), bodymask)
    push!(q.args, ifmask)
    push!(q.args, :dest)
    q
end
"""
    vmap!(f, destination, a::AbstractArray)
    vmap!(f, destination, a::AbstractArray, b::AbstractArray, ...)

Vectorized-`map!`, applying `f` to each element of `a` (or paired elements of `a`, `b`, ...)
and storing the result in `destination`.
"""
@generated function vmap!(f::F, dest::AbstractArray{T}, args::Vararg{<:AbstractArray,N}) where {F,T,N}
    # do not change argnames here without compensatory changes in vmap_quote
    vmap_quote(N, T)
end

function alignstores!(f::F, y::AbstractVector{T}, args::Vararg{<:Any,A}) where {F,T,A}
    N = length(y)
    ptry = pointer(y)
    ptrargs = pointer.(args)
    W = VectorizationBase.pick_vector_width(T)
    V = VectorizationBase.pick_vector_width_val(T)
    @assert iszero(reinterpret(UInt, ptry) & (sizeof(T) - 1)) "The destination vector (`dest`) must be aligned at least to `sizeof(eltype(dest))`."
    alignment = reinterpret(UInt, ptry) & (VectorizationBase.REGISTER_SIZE - 1)
    if alignment > 0
        i = reinterpret(Int, W - (alignment >>> VectorizationBase.intlog2(sizeof(T))))
        m = mask(T, i)
        if N < i
            m &= mask(T, N & (W - 1))
        end
        vstore!(ptry, extract_data(f(vload.(V, ptrargs, m)...)), m)
        gep(ptry, i), gep.(ptrargs, i), N - i
    else
        ptry, ptrargs, N
    end
end

"""
    vmapnt!(::Function, dest, args...)

This is a vectorized map implementation using nontemporal store operations. This means that the write operations to the destination will not go to the CPU's cache.
If you will not immediately be reading from these values, this can improve performance because the writes won't pollute your cache. This can especially be the case if your arguments are very long.

```julia
julia> using LoopVectorization, BenchmarkTools

julia> x = rand(10^8); y = rand(10^8); z = similar(x);

julia> f(x,y) = exp(-0.5abs2(x - y))
f (generic function with 1 method)

julia> @benchmark map!(f, \$z, \$x, \$y)
BenchmarkTools.Trial:
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     439.613 ms (0.00% GC)
  median time:      440.729 ms (0.00% GC)
  mean time:        440.695 ms (0.00% GC)
  maximum time:     441.665 ms (0.00% GC)
  --------------
  samples:          12
  evals/sample:     1

julia> @benchmark vmap!(f, \$z, \$x, \$y)
BenchmarkTools.Trial:
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     178.147 ms (0.00% GC)
  median time:      178.381 ms (0.00% GC)
  mean time:        178.430 ms (0.00% GC)
  maximum time:     179.054 ms (0.00% GC)
  --------------
  samples:          29
  evals/sample:     1

julia> @benchmark vmapnt!(f, \$z, \$x, \$y)
BenchmarkTools.Trial:
  memory estimate:  0 bytes
  allocs estimate:  0
  --------------
  minimum time:     144.183 ms (0.00% GC)
  median time:      144.338 ms (0.00% GC)
  mean time:        144.349 ms (0.00% GC)
  maximum time:     144.641 ms (0.00% GC)
  --------------
  samples:          35
  evals/sample:     1
```
"""
function vmapnt!(f::F, y::AbstractVector{T}, args::Vararg{<:Any,A}) where {F,T,A}
    ptry, ptrargs, N = alignstores!(f, y, args...)
    i = 0
    W = VectorizationBase.pick_vector_width(T)
    V = VectorizationBase.pick_vector_width_val(T)
    while i < N - ((W << 2) - 1)
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
    end
    while i < N - (W - 1) # stops at 16 when
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
    end
     if i < N
        m = mask(T, N & (W - 1))
        vstore!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i), m)...)), m)
    end
    y
end

"""
    vmapntt!(::Function, dest, args...)

Like `vmapnt!` (see `vmapnt!`), but uses `Threads.@threads` for parallel execution.
"""
function vmapntt!(f::F, y::AbstractVector{T}, args::Vararg{<:Any,A}) where {F,T,A}
    ptry, ptrargs, N = alignstores!(f, y, args...)
    N > 0 || return y
    W, Wshift = VectorizationBase.pick_vector_width_shift(T)
    V = VectorizationBase.pick_vector_width_val(T)
    Wsh = Wshift + 2
    Niter = N >>> Wsh
    Base.Threads.@threads for j ∈ 0:Niter-1
        i = j << Wsh
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...))); i += W
        vstorent!(gep(ptry, i), extract_data(f(vload.(V, gep.(ptrargs, i))...)))
    end
    ii = Niter << Wsh
    while ii < N - (W - 1) # stops at 16 when
        vstorent!(gep(ptry, ii), extract_data(f(vload.(V, gep.(ptrargs, ii))...))); ii += W
    end
    if ii < N
        m = mask(T, N & (W - 1))
        vstore!(gep(ptry, ii), extract_data(f(vload.(V, gep.(ptrargs, ii), m)...)), m)
    end
    y
end

function vmap_call(f::F, vm!::V, args::Vararg{<:Any,N}) where {V,F,N}
    T = Base._return_type(f, Base.Broadcast.eltypes(args))
    dest = similar(first(args), T)
    vm!(f, dest, args...)
end

"""
    vmap(f, a::AbstractArray)
    vmap(f, a::AbstractArray, b::AbstractArray, ...)

SIMD-vectorized `map`, applying `f` to each element of `a` (or paired elements of `a`, `b`, ...)
and returning a new array.
"""
vmap(f::F, args::Vararg{<:Any,N}) where {F,N} = vmap_call(f, vmap!, args...)

"""
    vmapnt(f, a::AbstractArray)
    vmapnt(f, a::AbstractArray, b::AbstractArray, ...)

A "non-temporal" variant of [`vmap`](@ref). This can improve performance in cases where
`destination` will not be needed soon.
"""
vmapnt(f::F, args::Vararg{<:Any,N}) where {F,N} = vmap_call(f, vmapnt!, args...)

"""
    vmapntt(f, a::AbstractArray)
    vmapntt(f, a::AbstractArray, b::AbstractArray, ...)

A threaded variant of [`vmapnt`](@ref).
"""
vmapntt(f::F, args::Vararg{<:Any,N}) where {F,N} = vmap_call(f, vmapntt!, args...)

# @inline vmap!(f, y, x...) = @avx y .= f.(x...)
# @inline vmap(f, x...) = @avx f.(x...)

ext: jl


text:
#= Atom Operations
Functions to extract information from OxygenAtoms.

=#

function valence_electrons(a::OxygenAtom)::Int
    sum(map(e -> e[end], a.orbitals.orbitals))
end

function neighbors(mol::OxygenMol, idx::Int)::Array{OxygenAtom,1}
    atom_neighbors = mol.adj[idx]
    [mol.atoms[i] for (i, _) in atom_neighbors]
end


text:
include("filtertest.jl")

a = [1.0 0.01;
     -0.01 1]
q = 1e-10*eye(2)
g = eye(2)

bfm = LinearModel(a,g,q)

hz = [1.0 0.0]
r = [0.1]'

bzm = LinearObservationModel(hz,r)

bkf = BasicKalmanFilter(State([1.0,0.0],4.0*eye(2)),bfm,bzm)

bkf1 = predict(bkf)
bkf2 = update(bkf1,y)

@test_approx_eq_eps bkf2.x.x kf.x.x 2e-10
@test_approx_eq_eps bkf2.x.p kf.x.p 2e-10


text:
__precompile__()

module RedisAlchemy

using Sockets
using Serialization

import Base: getindex, setindex!, wait, iterate, lastindex, length, sum,
             keys, values, sort, sort!, show, isempty, ==, |, &, ⊻, ~, +,
             close, size, push!, pushfirst!, pop!, popfirst!, read, write,
             string, seek, seekstart, seekend, eof, flush, open, read!,
             collect, in, wait, notify, resize!, put!, take!

abstract type AbstractRedisCollection end

function show(io::IO, rp::T) where T <: AbstractRedisCollection
    print(io, "$T(\"$(rp.key)\")")
end

include("util.jl")
include("exceptions.jl")
include("serializer.jl")

include("redis/connection.jl")
include("redis/resp.jl")
include("redis/exec.jl")

include("collections/bit.jl")
include("collections/blob.jl")
include("collections/condition.jl")
include("collections/counter.jl")
include("collections/dict.jl")
include("collections/list.jl")
include("collections/pipe.jl")
include("collections/string.jl")

end


text:
#=============================================================================#
#== The following is a tutorial on how you can use the debug.h5 debug       ==#
#== database. this demo uses Julia's HDF5 interface; but any language       ==#
#== that supports HDF5 can do this as well, albeit with different syntax.   ==#
#=============================================================================#

#== first, we need to load the HDF5 interface/module... ==#
using HDF5

#== ...and open the file ==#
debug = h5open("debug.h5", "r")

#=============================================================================#
#== The layout of information in each debug.h5 can be divided into two      ==#
#== parts - the pre-SCF information, and information for each SCF           ==#
#== iteration. Pre-SCF iteration information is contained in the            ==#
#== /RHF/Iteration-None H5 group, and information for SCF iteration ITER    ==#
#== is contained in the /RHF/Iteration-ITER H5 group.                       ==#
#=============================================================================#

#=============================================================================#
#== The /RHF/Iteration-None group contains the following datasets:          ==#
#==   - /S: the overlap matrix                                              ==#
#==   - /X: the orthogonalization/transformation matrix                     ==#
#==   - /Guess: the initial guess matrix                                    ==#
#==   - /E_nuc: the nuclear repulsion energy                                ==#
#==   - /H: the one-electron hamiltonian matrix                             ==#
#=============================================================================#

#=============================================================================#
#==  Let's access this debug.h5's overlap matrix and print it...            ==#
#=============================================================================#
println("debug.h5 overlap matrix:")
S_debug = debug["RHF"]["Iteration-None"]["S"][:]
display(S_debug); println(); println()

#=============================================================================#
#== Note that all matrices are stored as 1D arrays, and the full matrix     ==#
#== representation is stored.                                               ==#
#=============================================================================#

#=============================================================================#
#== The /RHF/Iteration-ITER group contains the following groups and         ==#
#== datasets:                                                               ==#
#==   - /C: the MO coefficients                                             ==#
#==   - /D: the updated density matrix                                      ==#
#==   - /E: a subgroup containing energy information, which includes:       ==#
#==     - /E/EHF1: the D*F subcomponent of the electronic energy            ==#
#==     - /E/EHF2: the D*H subcomponent of the electronic energy            ==#
#==     - /E/EHF: the total electronic energy                               ==#
#==   - /F: a subgroup containing Fock matrix information, which includes:  ==#
#==     - /F/Skeleton: the "skeleton" Fock matrix                           ==#
#==     - /F/Total: the "total" Fock matrix, including Hcore                ==#
#==   - /F_evec: the eigenvectors of the total Fock matrix                  ==#
#=============================================================================#

#=============================================================================#
#== Let's access this debug.h5's first-iteration MO coefficients matrix     ==#
#== and print it...                                                         ==#
#=============================================================================#
println("debug.h5 first-iteration MO coefficients matrix:")
C_debug = debug["RHF"]["Iteration-1"]["C"][:]
display(C_debug); println(); println()

#=============================================================================#
#== Now let's access this debug.h5's first-iteration total Fock matrix      ==#
#== and print it...                                                         ==#
#=============================================================================#
println("debug.h5 first-iteration total Fock matrix:")
F_total_debug = debug["RHF"]["Iteration-1"]["F"]["Total"][:]
display(F_total_debug); println(); println()

#=============================================================================#
#== And that's it! If you have any questions, let me know.                  ==#
#=============================================================================#
close(debug)
println("READ THE COMMENTS OF THIS DEBUG.JL FILE FOR A FULL TUTORIAL!")


text:
"""
    get_haplotype_transition_matrix(r, θ, α)

Compute transition matrices for the hidden Markov chains in haplotypes.
This is 2 equations above eq8 in "Gene hunting with hidden Markov model knockoffs" by Sesia et al.

# Inputs
`r`: Length `p` vector, the "recombination rates"
`θ`: Size `p × K` matrix, `θ[j, k]` is probability that the allele is 1 for SNP `p` at `k`th haplotype motif
`α`: Size `p × K` matrix, probabilities that haplotype motifs succeed each other. Rows should sum to 1.

# Output
`Q`: A `p`-dimensional vector of `K × K` matrices. `Q[:, :, j]` is the `j`th transition matrix.
"""
function get_haplotype_transition_matrix(
    r::AbstractVecOrMat, # p × 1
    θ::AbstractMatrix,   # p × K
    α::AbstractMatrix,   # p × K
    )
    K = size(θ, 2)
    p = size(r, 1)
    Q = Array{Float64, 3}(undef, K, K, p)
    @inbounds for j in 1:p
        Qj = @view(Q[:, :, j])
        for k in 1:K, knew in 1:K
            Qj[k, knew] = (1 - exp(-r[j])) * α[j, knew] # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
            if k == knew
                Qj[k, knew] += exp(-r[j])
            end
        end
    end
    return Q # Rows of Q should sum to 1
end

"""
    get_genotype_transition_matrix(r, θ, α, q, table)

Compute transition matrices for the hidden Markov chains in unphased genotypes.
This is equation 9 of "Gene hunting with hidden Markov model knockoffs" by Sesia et al.

# Inputs
`r`: Length `p` vector, the "recombination rates"
`θ`: Size `p × K` matrix, `θ[j, k]` is probability that the allele is 1 for SNP `p` at `k`th haplotype motif
`α`: Size `p × K` matrix, probabilities that haplotype motifs succeed each other. Rows should sum to 1.
`q`: Length `K` vector of initial probabilities
`table`: a `MarkovChainTable` that maps markov chain states k = 1, ..., K+(K+1)/2
    to haplotype pairs (ka, kb).
"""
function get_genotype_transition_matrix(
    r::AbstractVecOrMat, # p × 1
    θ::AbstractMatrix,   # p × K
    α::AbstractMatrix,   # p × K
    q::AbstractVector,   # p × 1
    table::MarkovChainTable
    )
    # first compute haplotype transition matrix
    H = get_haplotype_transition_matrix(r, θ, α)
    K = size(H, 2)
    p = size(H, 3)
    statespace = (K * (K + 1)) >> 1

    # now compute genotype transition matrix
    Q = Array{Float64, 3}(undef, statespace, statespace, p)
    @inbounds for l in 1:statespace
        Q[l, :, 1] .= q
    end
    # for l in 1:statespace
    #     Q[:, :, 1] .= NaN # Q1 should never be used anywhere, so we fill it with NaN
    # end
    @inbounds for j in 2:p
        Qj, Hj = @view(Q[:, :, j]), @view(H[:, :, j])
        for (row, geno) in enumerate(table)
            for (col, geno_new) in enumerate(table)
                Qj[row, col] = Hj[geno.a, geno_new.a] * Hj[geno.b, geno_new.b] # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
                if geno_new.a != geno_new.b
                    Qj[row, col] += Hj[geno.a, geno_new.b] * Hj[geno.b, geno_new.a]
                end
            end
        end
    end
    return Q #Rows of Q should sum to 1
end
# helper functions from HMMBase.jl
# istransmat(A::AbstractMatrix) =
#     issquare(A) && all([isprobvec(A[i, :]) for i = 1:size(A, 1)])
# @assert all(i -> istransmat(@view(Q[:, :, i])), 1:size(Q, 3))

function get_initial_probabilities(α::AbstractMatrix, table::MarkovChainTable)
    K = size(α, 2)
    statespace = (K * (K + 1)) >> 1
    q = zeros(statespace)
    α1 = α[1, :]
    @inbounds for (i, geno) in enumerate(table)
        ka, kb = geno.a, geno.b
        q[i] = (ka == kb ? abs2(α1[ka]) : 2 * α1[ka] * α1[kb])
    end
    @assert sum(q) ≈ 1 "initial probability should sum to 1!"
    return q
end

"""
    get_haplotype_emission_probabilities(θ::AbstractMatrix, j::Int, hj::Number, zj::Int)

Computes emission probabilities for unphased HMM. This is the equation above eq8 of
"Gene hunting with hidden Markov model knockoffs" by Sesia et al.
"""
function get_haplotype_emission_probabilities(θ::AbstractMatrix, j::Int, hj::Number, zj::Int)
    if hj == 0
        return 1 - θ[j, zj]
    elseif hj == 1
        return θ[j, zj]
    else
        error("hj should be 0 or 1 but was $hj")
    end
end

"""
    get_genotype_emission_probabilities(θ::AbstractMatrix, xj::Number, ka::Int, kb::Int, j::Int)

Computes P(xj | k={ka,kb}, θ): emission probabilities for genotypes. This is eq 10 of
"Gene hunting with hidden Markov model knockoffs" by Sesia et al.
"""
function get_genotype_emission_probabilities(θ::AbstractMatrix, xj::Number, ka::Int, kb::Int, j::Int)
    if xj == 0
        return (1 - θ[j, ka]) * (1 - θ[j, kb])
    elseif xj == 1
        return θ[j, ka] * (1 - θ[j, kb]) + θ[j, kb] * (1 - θ[j, ka])
    elseif xj == 2
        return θ[j, ka] * θ[j, kb]
    else
        error("xj should be 0, 1, or 2 but was $xj")
    end
end
# for j in 1:p, (k, (ka, kb)) in enumerate(with_replacement_combinations(1:K, 2))
#     p0 = get_genotype_emission_probabilities(θ, 0, ka, kb, j)
#     p1 = get_genotype_emission_probabilities(θ, 1, ka, kb, j)
#     p2 = get_genotype_emission_probabilities(θ, 2, ka, kb, j)
#     p0 + p1 + p2 ≈ 1 || error("shouldn't happen")
# end

"""
    form_emission_prob_matrix(a, θ, xi::AbstractVector)

# Inputs
+ `a`: `p × K` matrix with values estimated from fastPHASE (i.e. they called it the α parameter)
+ `θ`: `p × K` matrix with values estimated from fastPHASE
+ `xi`: Length `p` vector with sample `i`'s genotypes (entries 0, 1 or 2)
"""
function form_emission_prob_matrix(a, θ, xi::AbstractVector, table::MarkovChainTable)
    p, K = size(a)
    statespace = (K * (K + 1)) >> 1
    f = zeros(p, statespace)
    @inbounds for j in 1:p, (k, geno) in enumerate(table)
        f[j, k] = get_genotype_emission_probabilities(θ, xi[j], geno.a, geno.b, j)
    end
    return f
end

"""
    forward_backward_sampling!(Z, X, Q, q, θ, ...)

Samples Z, the hidden states of a HMM, from observed sequence of unphased genotypes X.

# Inputs
`Z`: Length `p` vector of integers. This will store the sampled Markov states
`X`: Length `p` vector of genotypes (0, 1, or 2)
`Q`: `K × K × p` array. `Q[:, :, j]` is a `K × K` matrix of transition
    probabilities for `j`th state, i.e. Q[l, k, j] = P(X_{j} = k | X_{j - 1} = l).
    The first transition matrix is not used.
`q`: Length `K` vector of initial probabilities
`θ`: The θ parameter estimated from fastPHASE

# Preallocated storage variables
`table`: a `MarkovChainTable` that maps markov chain states to haplotype
    pairs (ka, kb)
`d`: Sampling distribution, probabilities in d.p are mutated
`α̂`: `p × K` scaled forward probability matrix, where
    `α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)`
`c`: normalizing constants, `c[k] = p(x_k | x_1,...,x_{k-1})`

# Reference
Algorithm 3 of "Gene hunting with hidden Markov model knockoffs" by Sesia et al
"""
function forward_backward_sampling!(
    Z::Vector{Int},
    X::Vector,
    Q::Array{T, 3},
    q::Vector{T},
    θ::AbstractMatrix,
    table::MarkovChainTable,
    d::Categorical,
    α̂::AbstractMatrix,
    c::AbstractVector,
    ) where T
    statespace, p = size(Q, 2), size(Q, 3)
    length(X) == p || error("length(X) not equal to p")
    fill!(c, 0)

    # (scaled) forward probabilities
    @inbounds for (k, geno) in enumerate(table)
        α̂[1, k] = q[k] * get_genotype_emission_probabilities(θ, X[1], geno.a, geno.b, 1)
        c[1] += α̂[1, k]
    end
    α̂[1, :] ./= c[1]
    @inbounds for j in 2:p
        mul!(@view(α̂[j, :]), Transpose(@view(Q[:, :, j])), @view(α̂[j - 1, :])) # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
        for (k, geno) in enumerate(table)
            α̂[j, k] *= get_genotype_emission_probabilities(θ, X[j], geno.a, geno.b, j)
            c[j] += α̂[j, k]
        end
        α̂[j, :] ./= c[j]
    end

    # backwards sampling
    denom = sum(@view(α̂[p, :]))
    @inbounds for k in 1:statespace
        d.p[k] = α̂[p, k] / denom
    end
    Z[end] = rand(d)
    @inbounds for j in Iterators.reverse(1:p-1)
        denom = 0.0
        for k in 1:statespace
            denom += Q[k, Z[j + 1], j + 1] * α̂[j, k]
        end
        for zj in 1:statespace
            d.p[zj] = Q[zj, Z[j + 1], j + 1] * α̂[j, zj] / denom
        end
        @assert sum(d.p) ≈ 1 "forward_backward_sampling!: probability should sum to 1 but was $(sum(d.p))"
        Z[j] = rand(d)
    end

    return Z
end
# todo: how to test correctness?

function forward_backward_sampling(
    X::Vector,
    Q::Array{T, 3},
    q::Vector{T},
    θ::AbstractMatrix
    ) where T
    p = length(X)
    K = size(Q, 1)
    Z = zeros(Int, p)
    table = MarkovChainTable(K)
    statespace = statespace(table)
    d = Categorical([1 / statespace for _ in 1:statespace])
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})
    forward_backward_sampling!(Z, X, Q, q, θ, table, d, α̂, c)
end

"""
    hmm_knockoff(plinkname, fastphase_outfile, T=10, datadir=pwd())

Generates HMM knockoffs from binary PLINK formatted files. This is done by
first running fastPHASE, then running Algorithm 2 of "Gene hunting with hidden
Markov model knockoffs" by Sesia, Sabatti, and Candes

# Input
+ `plinkname`: Binary PLINK file names without the `.bed/.bim/.fam` suffix.

# Optional arguments
+ `T`: Number of initial starts used in fastPHASE EM algorithm (default = number
    of parallel julia threads available as measured by Threads.nthreads())
+ `datadir`: Full path to the PLINK and fastPHASE files (default = current directory)
+ `plink_outfile`: Output PLINK format name
+ `fastphase_outfile`: The output file name from fastPHASE's alpha, theta, r files
+ `args...`: Any parameter specified in `fastphase`

# Output
+ `plink_outfile.bed`: `n × p` knockoff genotypes
+ `plink_outfile.bim`: SNP mapping file. Knockoff have SNP names ending in ".k"
+ `plink_outfile.fam`: Sample mapping file, this is a copy of the original `plinkname.fam` file
+ `fastphase_outfile_rhat.txt`: averaged r hat file from fastPHASE
+ `fastphase_outfile_alphahat.txt`: averaged alpha hat file from fastPHASE
+ `fastphase_outfile_thetahat.txt`: averaged theta hat file from fastPHASE
"""
function hmm_knockoff(
    plinkname::AbstractString;
    T::Int = Threads.nthreads(),
    datadir::AbstractString = pwd(),
    plink_outfile::AbstractString = "knockoff",
    fastphase_outfile::AbstractString = "fastphase_out",
    outdir::AbstractString = datadir,
    args...
    )
    snpdata = SnpData(joinpath(datadir, plinkname))
    r, θ, α = fastphase_estim_param(snpdata; out=fastphase_outfile, args...)
    return hmm_knockoff(snpdata, r, θ, α, outdir=outdir, plink_outfile=plink_outfile)
end

"""
    hmm_knockoff(snpdata, r, θ, α)

Generates knockoff of `snpdata` with loaded r, θ, α
"""
function hmm_knockoff(
    snpdata::SnpData,
    r::AbstractVecOrMat,
    θ::AbstractMatrix,
    α::AbstractMatrix;
    outdir = pwd(),
    plink_outfile::AbstractString = "knockoff",
    )
    Xfull = snpdata.snparray
    n, p = size(Xfull)
    K = size(θ, 2)
    statespace = (K * (K + 1)) >> 1
    table = MarkovChainTable(K)

    # get initial states (marginal distribution vector) and Markov transition matrices
    q = get_initial_probabilities(α, table)
    Q = get_genotype_transition_matrix(r, θ, α, q, table)

    # preallocated arrays
    X̃full = SnpArray(joinpath(outdir, plink_outfile * ".bed"), n, p)
    X = zeros(Float64, p)
    Z = zeros(Int, p)
    Z̃ = zeros(Int, p)
    X̃ = zeros(Int, p)
    N = zeros(p, statespace)
    d_K = Categorical([1 / statespace for _ in 1:statespace]) # for sampling markov chains (length statespace)
    d_3 = Categorical([1 / statespace for _ in 1:statespace]) # for sampling genotypes (length 3)
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})

    @showprogress for i in 1:n
        # sample hidden states (algorithm 3 in Sesia et al)
        copyto!(X, @view(Xfull[i, :]))
        forward_backward_sampling!(Z, X, Q, q, θ, table, d_K, α̂, c)

        # sample knockoff of markov chain (algorithm 2 in Sesia et al)
        markov_knockoffs!(Z̃, Z, N, d_K, Q, q)

        # sample knockoffs of genotypes (eq 6 in Sesia et al)
        sample_markov_chain!(X̃, Z̃, table, θ, d_3)

        # save knockoff
        write_plink!(X̃full, X̃, i)
    end

    # copy .bim and .fam files
    new_bim = copy(snpdata.snp_info)
    for i in 1:p
        new_bim[i, :snpid] = new_bim[i, :snpid] * ".k"
    end
    CSV.write(joinpath(outdir, plink_outfile * ".bim"), new_bim, delim='\t', header=false)
    cp(snpdata.srcfam, joinpath(outdir, plink_outfile * ".fam"), force=true)

    return X̃full
end

function hmm_knockoff(
    plinkname::AbstractString,
    fastphase_outfile::AbstractString;
    T::Int = Threads.nthreads(),
    datadir::AbstractString = pwd(),
    outfile::AbstractString = "knockoff",
    outdir::AbstractString = datadir
    )
    snpdata = SnpData(joinpath(datadir, plinkname))
    Xfull = snpdata.snparray
    n, p = size(Xfull)

    # get r, α, θ estimated by fastPHASE
    r, θ, α = process_fastphase_output(datadir, T=T, extension=fastphase_outfile)
    K = size(θ, 2)
    statespace = (K * (K + 1)) >> 1
    table = MarkovChainTable(K)

    # get initial states (marginal distribution vector) and Markov transition matrices
    q = get_initial_probabilities(α, table)
    Q = get_genotype_transition_matrix(r, θ, α, q, table)

    # preallocated arrays
    X̃full = SnpArray(joinpath(outdir, outfile * ".bed"), n, p)
    X = zeros(Float64, p)
    Z = zeros(Int, p)
    Z̃ = zeros(Int, p)
    X̃ = zeros(Int, p)
    N = zeros(p, statespace)
    d_K = Categorical([1 / statespace for _ in 1:statespace]) # for sampling markov chains (length statespace)
    d_3 = Categorical([1 / statespace for _ in 1:statespace]) # for sampling genotypes (length 3)
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})

    @showprogress for i in 1:n
        # sample hidden states (algorithm 3 in Sesia et al)
        copyto!(X, @view(Xfull[i, :]))
        forward_backward_sampling!(Z, X, Q, q, θ, table, d_K, α̂, c)

        # sample knockoff of markov chain (algorithm 2 in Sesia et al)
        markov_knockoffs!(Z̃, Z, N, d_K, Q, q)

        # sample knockoffs of genotypes (eq 6 in Sesia et al)
        sample_markov_chain!(X̃, Z̃, table, θ, d_3)

        # save knockoff
        write_plink!(X̃full, X̃, i)
    end

    # copy .bim and .fam files
    new_bim = copy(snpdata.snp_info)
    for i in 1:p
        new_bim[i, :snpid] = new_bim[i, :snpid] * ".k"
    end
    CSV.write(joinpath(outdir, outfile * ".bim"), new_bim, delim='\t', header=false)
    cp(joinpath(datadir, plinkname * ".fam"), joinpath(outdir, outfile * ".fam"), force=true)

    return X̃full
end

function genotype_knockoffs(
    Z̃::AbstractVector,
    table::MarkovChainTable,
    θ::AbstractMatrix
    )
    p = length(Z̃)
    X̃ = zeros(eltype(Z̃), p)
    d = Categorical([1/3 for _ in 1:3])
    return sample_markov_chain!(X̃, Z̃, table, θ, d)
end

function sample_markov_chain!(
    X̃::AbstractVector,
    Z̃::AbstractVector,
    table::MarkovChainTable,
    θ::AbstractMatrix,
    d::Categorical # Categorical distribution from Distributions.jl
    )
    p = length(Z̃)
    @inbounds for j in 1:p
        a, b = index_to_pair(table, Z̃[j])
        d.p[1] = get_genotype_emission_probabilities(θ, 0, a, b, j)
        d.p[2] = get_genotype_emission_probabilities(θ, 1, a, b, j)
        d.p[3] = get_genotype_emission_probabilities(θ, 2, a, b, j)
        X̃[j] = rand(d)
    end
    X̃ .-= 1 # sampling d returns states 1~3, but genotypes are 0~2
    return X̃
end

function write_plink!(
    X̃full::SnpArray,
    X̃::AbstractVector,
    i::Int # sample index
    )
    p = length(X̃)
    @inbounds for j in 1:p
        if X̃[j] == 0
            X̃full[i, j] = 0x00
        elseif X̃[j] == 1
            X̃full[i, j] = 0x02
        elseif X̃[j] == 2
            X̃full[i, j] = 0x03
        else
            error("Genotypes should only be 0, 1, or 2 but got $(X̃[j])")
        end
    end
    return X̃full
end


text:
# TODO compress Union{Int, Missing} is no missing present

# TODO allow int to uint
"""
	type_compress!(df, compress_float = false, verbose = false)

Compress a DataFrame by using types of smaller bit. If safe to do so, it will
"downgrade" `Int*` and `UInt*` if safe to do some. It will compress any
`CategoricalVector` with `DataFrames.compress`.

For `Vector{String}`, if the number unique values is less than 2^16 then it will
be converted to `CategoricalVector` and otherwise will be stored as
`WeakRefStrings.StringVector`.

If `compress_float = true` then `Float64` will be downgraded to `Float32`; but
beware that this means the calculation will be done with reduce precision.
"""
type_compress!(df; compress_float = false, verbose = false) = begin
    @assert Tables.istable(df)
    for n in names(df)
        if verbose
            println("Compressing $n")
        end
        if compress_float || (nonmissingtype(eltype(df[!, n])) != Float64)
            df[!, n] = type_compress(df[!, n])
        end
    end
    df
end

type_compress(v::Vector{T}) where {T<:Union{Int128,Int64,Int32,Int16}} = begin
    min1, max1 = extrema(v)

    if typemin(Int8) <= min1 && max1 <= typemax(Int8)
        return Int8.(v)
    elseif typemin(Int16) <= min1 && max1 <= typemax(Int16)
        return Int16.(v)
    elseif typemin(Int32) <= min1 && max1 <= typemax(Int32)
        return Int32.(v)
    elseif typemin(Int64) <= min1 && max1 <= typemax(Int64)
        return Int64.(v)
    end
end

type_compress(v::Vector{T}) where {T<:Union{UInt128,UInt64,UInt32,UInt16}} = begin
    max1 = maximum(v)

    if max1 <= typemax(UInt8)
        return UInt8.(v)
    elseif max1 <= typemax(UInt16)
        return UInt16.(v)
    elseif max1 <= typemax(UInt32)
        return UInt32.(v)
    elseif max1 <= typemax(UInt64)
        return UInt64.(v)
    end
end

type_compress(v::Vector{Union{Missing,T}}) where {T<:Union{UInt128,UInt64,UInt32,UInt16}} =
    begin
        max1 = maximum(skipmissing(v))

        if max1 <= typemax(UInt8)
            return Vector{Union{Missing,UInt8}}(v)
        elseif max1 <= typemax(UInt16)
            return Vector{Union{Missing,UInt16}}(v)
        elseif max1 <= typemax(UInt32)
            return Vector{Union{Missing,UInt32}}(v)
        elseif max1 <= typemax(UInt64)
            return Vector{Union{Missing,UInt64}}(v)
        end
    end

type_compress(v::Vector{Union{Missing,T}}) where {T<:Union{Int128,Int64,Int32,Int16}} =
    begin
        min1, max1 = extrema(skipmissing(v))

        if typemin(Int8) <= min1 && max1 <= typemax(Int8)
            return Vector{Union{Missing,Int8}}(v)
        elseif typemin(Int16) <= min1 && max1 <= typemax(Int16)
            return Vector{Union{Missing,Int16}}(v)
        elseif typemin(Int32) <= min1 && max1 <= typemax(Int32)
            return Vector{Union{Missing,Int32}}(v)
        elseif typemin(Int64) <= min1 && max1 <= typemax(Int64)
            return Vector{Union{Missing,Int64}}(v)
        end
    end

type_compress(v::Vector{Float64}) = Vector{Float32}(v)
type_compress(v::Vector{Union{Missing,Float64}}) = Vector{Union{Missing,Float32}}(v)

type_compress(v::Vector{String}) = begin
    # TODO recommend a PooledString if necessary
    # use some heuristic to decide whether to compress

    ## if the number of unique elements is predicted to be less than typemax(UInt16) + 1
    ## then we can use categorical array to compress them
    # sv = countmap(sample(v, 888))
    #
    # # Estimate the number of unique items
    # # https://stats.stackexchange.com/questions/19014/how-can-i-estimate-unique-occurrence-counts-from-a-random-sampling-of-data
    # u1 = length([key for (key, value) in sv if value == 1])
    # u = length(sv)
    # # if the estimated number of estimtes is less than about 32000 then it's
    # # worth putting it into a categorical array
    # if u + u1/888 * (length(v) - 888) < typemax(UInt16) + 1
    # 	return categorical(v)
    # 	# rlev = rle(categorical(v))
    # 	# rlev[2] .= cumsum(rlev[2])
    # 	# return RLEVector(rlev...)
    # end

    if length(Set(v)) <= 2^16
        return categorical(v)
    end

    # check if the string is more compact at RLE level
    rlev = rle(v)

    # firstly check if the rlev can be whittle down further
    offset_dict = Dict{String,UInt}()

    # an IOBuffer to build up the string buffer
    long_string_builder = IOBuffer()
    offset_array_builder = IOBuffer()
    string_len_builder = IOBuffer()

    values = rlev[1]
    offset_upto = 0

    # add the first entry
    offset_dict[values[1]] = offset_upto

    write(offset_array_builder, offset_upto)
    offset_update = write(long_string_builder, values[1])
    write(string_len_builder, offset_update)

    offset_upto += offset_update

    for v in values[2:end]
        if !haskey(offset_dict, v)
            write(offset_array_builder, offset_upto)
            offset_update = write(long_string_builder, v)
            write(string_len_builder, offset_update)

            # update the offset_upto for next one
            offset_dict[v] = offset_upto
            offset_upto += offset_update
            # println(v, " : ", offset_dict[v])
        else
            # If the string doesn't existing then obtain the offset from the
            # offset_dict.
            # There is no need to write it to the string aagain because it's
            # already there

            offset_old = offset_dict[v]
            # still write the lengths
            write(string_len_builder, sizeof(v))
            # still write the offset
            write(offset_array_builder, offset_old)
        end
    end

    #long_str =String(take!(long_string_builder))
    offsets = inverse_rle(reinterpret(Int, take!(offset_array_builder)), rlev[2])
    lengths = inverse_rle(reinterpret(Int, take!(string_len_builder)), rlev[2])

    buffer = take!(long_string_builder)

    close(long_string_builder)
    close(offset_array_builder)
    close(string_len_builder)

    #return String(buffer), offsets, lengths
    #return (StringArray{String, 1}(buffer, offsets, lengths), String(buffer), offsets, lengths)
    return StringArray{String,1}(buffer, offsets, lengths)

end

type_compress(v::Vector{Missing}) = v

type_compress(v::CategoricalVector) where {T,IntType} = compress(v)

type_compress(v) = begin
    # println("The compression for $(typeof(v)) is not yet supported. No compression is performed. Submit an idea if you think JDF should support it: https://github.com/xiaodaigh/JDF.jl/issues")
    v
end

if false

end

max_stars_repo_name: xiaodaigh/JDF

max_issues_repo_name: xiaodaigh/JDF

max_forks_repo_name: xiaodaigh/JDF


text:
@mpi_handle Comm

const COMM_NULL = _Comm(MPI_COMM_NULL)
const COMM_WORLD = _Comm(MPI_COMM_WORLD)
const COMM_SELF = _Comm(MPI_COMM_SELF)

Comm() = Comm(COMM_NULL.val)

function free(comm::Comm)
    if comm.val != COMM_NULL.val
        @mpichk ccall((:MPI_Comm_free, libmpi), Cint, (Ptr{MPI_Comm},), comm)
        refcount_dec()
    end
    return nothing
end

"""
    Comm_rank(comm:Comm)

The rank of the process in the particular communicator's group.

Returns an integer in the range `0:MPI.Comm_size()-1`.
"""
function Comm_rank(comm::Comm)
    rank = Ref{Cint}()
    @mpichk ccall((:MPI_Comm_rank, libmpi), Cint, (MPI_Comm, Ptr{Cint}), comm, rank)
    Int(rank[])
end

"""
    Comm_size(comm:Comm)

The number of processes involved in communicator.
"""
function Comm_size(comm::Comm)
    size = Ref{Cint}()
    @mpichk ccall((:MPI_Comm_size, libmpi), Cint, (MPI_Comm, Ptr{Cint}), comm, size)
    Int(size[])
end

function Comm_dup(comm::Comm)
    newcomm = Comm()
    @mpichk ccall((:MPI_Comm_dup, libmpi), Cint, (MPI_Comm, Ptr{MPI_Comm}), comm, newcomm)
    refcount_inc()
    finalizer(free, newcomm)
    newcomm
end

function Comm_split(comm::Comm, color::Integer, key::Integer)
    newcomm = Comm()
    @mpichk ccall((:MPI_Comm_split, libmpi), Cint,
        (MPI_Comm, Cint, Cint, Ptr{MPI_Comm}), comm, color, key, newcomm)
    refcount_inc()
    finalizer(free, newcomm)
    newcomm
end

function Comm_split_type(comm::Comm,split_type::Integer,key::Integer; kwargs...)
    newcomm = Comm()
    @mpichk ccall((:MPI_Comm_split_type, libmpi), Cint,
          (MPI_Comm, Cint, Cint, MPI_Info, Ptr{MPI_Comm}),
          comm, split_type, key, Info(kwargs...), newcomm)
    refcount_inc()
    finalizer(free, newcomm)
    newcomm
end

function Comm_get_parent()
    comm = Comm()
    @mpichk ccall((:MPI_Comm_get_parent, libmpi), Cint, (Ptr{MPI_Comm},), comm)
    comm
end

function Comm_spawn(command::String, argv::Vector{String}, nprocs::Integer,
                    comm::Comm, errors = Vector{Cint}(undef, nprocs); kwargs...)
    intercomm = Comm()
    # int MPI_Comm_spawn(const char *command, char *argv[], int maxprocs,
    #                    MPI_Info info, int root, MPI_Comm comm, MPI_Comm *intercomm,
    #                    int array_of_errcodes[])
    @mpichk ccall((:MPI_Comm_spawn, libmpi), Cint,
         (Cstring, Ptr{Ptr{Cchar}}, Cint, MPI_Info, Cint, MPI_Comm, Ptr{MPI_Comm}, Ptr{Cint}),
          command, argv, nprocs, Info(kwargs...), 0, comm, intercomm, errors)
    refcount_inc()
    finalizer(free, intercomm)
    return intercomm
end

function Intercomm_merge(intercomm::Comm, flag::Bool)
    newcomm = Comm()
    @mpichk ccall((:MPI_Intercomm_merge, libmpi), Cint,
        (MPI_Comm, Cint, Ptr{MPI_Comm}), intercomm, Cint(flag), newcomm)
    refcount_inc()
    finalizer(free, newcomm)
    return newcomm
end

"""
    universe_size()

The total number of available slots, or `nothing` if it is not defined. This is determined by the `MPI_UNIVERSE_SIZE` attribute of `COMM_WORLD`.

This is typically dependent on the MPI implementation: for MPICH-based implementations, this is specified by the `-usize` argument. OpenMPI defines a default value based on the number of processes available.
"""
function universe_size()
    flag = Ref{Cint}()
    result = Ref(Ptr{Cint}(C_NULL))
    # int MPI_Comm_get_attr(MPI_Comm comm, int comm_keyval, void *attribute_val, int *flag)
    @mpichk ccall((:MPI_Comm_get_attr, libmpi), Cint,
        (MPI_Comm, Cint, Ptr{Cvoid}, Ptr{Cint}), MPI.COMM_WORLD, MPI_UNIVERSE_SIZE, result, flag)
    if flag[] == 0
        return nothing
    end
    Int(unsafe_load(result[]))
end


text:
files = Dict("EARTH" => "VSOP87E.ear",
             "JUPITER" => "VSOP87E.jup",
             "MARS" => "VSOP87E.mar",
             "MERCURY" => "VSOP87E.mer",
             "NEPTUNE" => "VSOP87E.nep",
             "SATURN" => "VSOP87E.sat",
             "SUN" => "VSOP87E.sun",
             "URANUS" => "VSOP87E.ura",
             "VENUS" => "VSOP87E.ven")

coeffs = (x=[], y=[], z=[])

indent = repeat(" ", 4)

for (name, f) in files
    lines = open(readlines, f)
    for line in lines
        startswith(line, " VSOP") && continue

        idx = parse(Int, line[4:4])
        order = parse(Int, line[5:5]) + 1
        if length(coeffs[idx]) < order
            push!(coeffs[idx], (amplitude=[], phase=[], frequency=[]))
        end
        push!(coeffs[idx][order][:amplitude], parse(Float64, line[80:97]))
        push!(coeffs[idx][order][:phase], parse(Float64, line[98:111]))
        push!(coeffs[idx][order][:frequency], parse(Float64, line[112:131]))
    end

    open("vsop87_$name.jl", "w") do f
        for (idx, coord) in enumerate(["X", "Y", "Z"])
            write(f, "const $(name)_$(coord)_NUM = (\n")
            for (order, co) in enumerate(coeffs[idx])
                write(f, "$indent$(length(co[1])),\n")
            end
            write(f, ")\n\n")
            for (i, co) in enumerate(coeffs[idx])
                order = i - 1
                write(f, "const $(name)_$(coord)_$(order)_AMP = [\n")
                for c in co[1]
                    write(f, "$indent$c,\n")
                end
                write(f, "]\n\n")
                write(f, "const $(name)_$(coord)_$(order)_PHS = [\n")
                for c in co[2]
                    write(f, "$indent$c,\n")
                end
                write(f, "]\n\n")
                write(f, "const $(name)_$(coord)_$(order)_FRQ = [\n")
                for c in co[3]
                    write(f, "$indent$c,\n")
                end
                write(f, "]\n\n")
            end
        end

        write(f, "const VSOP_$(name) = ((\n")
        for (idx, coord) in enumerate(["X", "Y", "Z"])
            for i in 0:length(coeffs[idx]) - 1
                write(f, "$indent($(name)_$(coord)_$(i)_AMP, $(name)_$(coord)_$(i)_PHS, $(name)_$(coord)_$(i)_FRQ),\n")
            end
            if idx in (1, 2)
                write(f, "), (\n")
            end
        end
        write(f, "))\n\n")
        write(f, "const VSOP_$(name)_NUM = ($(name)_X_NUM, $(name)_Y_NUM, $(name)_Z_NUM)\n\n")
    end
    empty!(coeffs[1])
    empty!(coeffs[2])
    empty!(coeffs[3])
end


text:
function reach_ASB07!(R::Vector{<:ReachSet},
                      Ω0::LazySet,
                      U::Union{ConstantInput, Nothing},
                      Φ::AbstractMatrix,
                      N::Int,
                      δ::Float64,
                      max_order::Int)
    # initial reach set
    t0, t1 = zero(δ), δ
    R[1] = ReachSet(Ω0, t0, t1)

    k = 2
    while k <= N
        Rₖ = overapproximate(Φ * R[k-1].X, Zonotope)
        if U != nothing
            Rₖ = minkowski_sum(Rₖ, next_set(U))
        end
        Rₖ = reduce_order(Rₖ, max_order)  # reduce order

        # store reach set
        t0 = t1
        t1 += δ
        if ! (genmat(Rₖ) isa Matrix)
            Rₖ = Zonotope(center(Rₖ), Matrix(genmat(Rₖ)))
        end
        R[k] = ReachSet(Rₖ, t0, t1)

        k += 1
    end
    return R
end


text:
# algorithm 4 in section 5.4 of https://www.annualreviews.org/doi/10.1146/annurev-conmatphys-031016-025507 applied to cMPO

struct qbimps
    A::TensorMap{ComplexSpace, 2, 1}
    B::cmps
end

function qbimps(f, chi::Integer, dp::Integer, dv::Integer)
    A = TensorMap(f, ComplexF64, ℂ^chi*ℂ^dp, ℂ^chi)
    B = cmps(f, chi, dv)
    return qbimps(A, B)
end

function A_canonical(T::cmpo, psi::qbimps)
    _, AR = right_canonical_QR(psi.A, 1e-12)

    chi = get_chi(psi.B)
    d = get_d(psi.B)

    BR_R = TensorMap(rand, ComplexF64, ℂ^chi*ℂ^d, ℂ^chi)
    BR_Q = TensorMap(rand, ComplexF64, ℂ^chi, ℂ^chi)

    function lopR(vR::TensorMap{ComplexSpace, 2, 1})
        @tensor TvR[-1, -2; -3] := vR[1, 3, 4] * AR[-1, 2, 1] * T.P[-2, 5, 2, 3] * AR'[4, -3, 5] +
                                   (-1.0) * vR[-1, -2, -3]
        return TvR
    end
    @tensor bR[-1, -2; -3] := AR[-1, 2, 1] * T.R[3, -2, 2] * AR'[1, -3, 3]
    BR_R, _ = linsolve(lopR, -bR, BR_R)

    lopQ = transf_mat(AR, AR)
    lopQ_T = transf_mat_T(AR, AR)
    @tensor bQ[-1; -2] := BR_R[1, 3, 4] * AR[-1, 2, 1] * AR'[4, -2, 5] * T.L'[5, 2, 3] +
                          AR[-1, 1, 2] * T.Q[3, 1] * AR'[2, -2, 3]
    Id_Q = id(ℂ^chi)
    _, vQL = eigsolve(lopQ_T, Id_Q, 1)
    vQL = vQL[1] / tr(vQL[1] * Id_Q')
    BR_Q, _ = linsolve(v -> lopQ(v)-tr(vQL'*v)*Id_Q-v, -bQ, BR_Q)

    # improve the linsolve result by a few more power iterations
    ix, δ = 0, 999
    while δ > 1e-12 && ix < 100
        BR_Q1 = lopQ(BR_Q) - tr(vQL'*BR_Q)*Id_Q + bQ
        δ = (BR_Q1 - BR_Q).data / chi*2 |> norm
        BR_Q = BR_Q1
        ix += 1
    end

    Λ = - tr(vQL' * BR_Q)
    (δ > 1e-12) && @warn "fixed point equation not fully converged for psi.B. δ=$δ"

    BR = cmps(BR_Q, BR_R)

    #println("A canonical. Λ = $Λ")
    return Λ, qbimps(AR, BR)
end

function B_canonical(T::cmpo, psi::qbimps)
    _, BL = left_canonical(psi.B)

    #Xinv = inv(X)
    #@tensor AL[-1, -2; -3] := psi.A[1, -2, 2] * X[-1, 1] * Xinv[2, -3]
    chi, d = get_chi(psi.A), get_d(psi.A)
    AL = TensorMap(rand, ComplexF64, ℂ^chi*ℂ^d, ℂ^chi)

    function lop(v::TensorMap{ComplexSpace, 2, 1})
        @tensor Tv[-1, -2; -3] :=
            v[-1, -2, 1] * BL.Q[1, -3] +
            v[-1, 1, -3] * T.Q[-2, 1] +
            v[1, -2, -3] * BL.Q'[-1, 1] +
            v[-1, 1, 2] * T.L'[-2, 1, 3] * BL.R[2, 3, -3] +
            v[1, 2, -3] * T.R[-2, 3, 2] * BL.R'[-1, 1, 3] +
            v[1, 2, 4] * BL.R[4, 5, -3] * T.P[3, -2, 2, 5] * BL.R'[-1, 1, 3]
        return Tv
    end

    w, AL = eigsolve(lop, AL, 1, :LR)
    AL = AL[1]
    Λ = -w[1]
    #println("B canonical. Λ = $Λ")

    return Λ, qbimps(AL, BL)
end

expand(psi::qbimps, chi::Integer, perturb::Float64) = qbimps(expand(psi.A, chi, perturb), expand(psi.B, chi, perturb))


text:
export
ruleSPExponentialOutNG,
ruleSPExponentialOutNP,
ruleSPExponentialIn1LN,
ruleSPExponentialIn1PN

function ruleSPExponentialOutNG(msg_out::Nothing,
								msg_in1::Message{F, Univariate}) where F<:Gaussian

    d_in1 = convert(ProbabilityDistribution{Univariate, GaussianMeanVariance}, msg_in1.dist)

    return Message(Univariate, LogNormal, m=d_in1.params[:m], s=d_in1.params[:v])
end

ruleSPExponentialIn1LN(msg_out::Message{LogNormal, Univariate}, msg_in1::Nothing) =
	Message(Univariate, GaussianMeanVariance, m=msg_out.dist.params[:m], v=msg_out.dist.params[:s])

ruleSPExponentialOutNP(msg_out::Nothing, msg_in1::Message{PointMass, Univariate}) =
	Message(Univariate, PointMass, m=exp(msg_in1.dist.params[:m]))

ruleSPExponentialIn1PN(msg_out::Message{PointMass, Univariate}, msg_in1::Nothing) =
	Message(Univariate, PointMass, m=log(msg_out.dist.params[:m]))


text:
struct AcrobotParams{T} <:Any where {T<:Real}
    m1::T 	# mass of first link
    m2::T 	# mass of second link
    l1::T 	# length of first link
	l2::T 	# length of second link
	lc1::T 	# position of center mass of link1
	lc2::T 	# position of center mass of link2
	i1::T  	# link1 moment of inertia
	i2::T  	# link2 moment of inertia
	g::T   	# gravity force (not directional)
	fmag::T # max force magnitude

    AcrobotParams() = new{Float64}(1., 1., 1., 1., 0.5, 0.5, 1., 1., 9.8, 1.)
    AcrobotParams(T::Type) = new{T}(1., 1., 1., 1., 0.5, 0.5, 1., 1., 9.8, 1.)
	AcrobotParams(T::Type, m1, m2, l1, l2, g) = new{T}(m1, m2, l1, l2, 0.5*l1, 0.5*l2, 1., 1., g, 1.)
end

function acrobot_finitetime(;randomize=false, maxT=400.0, dt=0.2, Atype=:Discrete, droptime=true, stochastic_start=false)
	if randomize
		params = random_acrobot_params()
	else
		params = AcrobotParams()
	end
	return create_finitetime_acrobot(params; maxT=maxT, dt=dt, Atype=Atype, droptime=droptime, stochastic_start=stochastic_start)
end

function create_finitetime_acrobot(params::AcrobotParams; maxT=400.0, dt=0.2, Atype=:Discrete, droptime=true, stochastic_start=false)
    X = zeros((4,2))
    X[1,:] .= [-π, π]               # theta1 range
	X[2,:] .= [-π, π]               # theta2 range
	X[3,:] .= [-4. * π, 4. * π]     # theta1Dot range
	X[4,:] .= [-9. * π, 9. * π]     # theta2Dot range
    S = ([0. maxT],				# time range
		X)           	# thetaDot range

	if Atype==:Discrete
		A = 1:3
	else
		A = [-1.0 1.0]
    end

	if droptime
		X = S[2]
		function get_outcome1(s,a,params,dt,maxT,stochastic_start)
			t, x, r, γ = acrobot_step!(s,a, params, dt, maxT, stochastic_start)
			s = (t,x)
			return s,x,r,γ
		end
        p = (s,a)->get_outcome1(s,a,params,dt,maxT,stochastic_start)

        function abd0_obs()
            t,x = acrobot_sample_initial(stochastic_start)
            return (t,x), x
        end
        d0 = abd0_obs
	else
		X = S
		function get_outcome2(s,a,params,dt,maxT,stochastic_start)
			t, x, r, γ = acrobot_step!(s,a, params, dt, maxT,stochastic_start)
			s = (t,x)
			return s,s,r,γ
		end
        p = (s,a)->get_outcome2(s,a,params,dt,maxT,stochastic_start)
        function abd0()
            s = acrobot_sample_initial(stochastic_start)
            return s,s
        end
        d0 = abd0
    end

	meta = Dict{Symbol,Any}()
    meta[:minreward] = -0.1
    meta[:maxreward] = 0.0
    meta[:minreturn] = -ceil(Int, maxT / dt)
    meta[:maxreturn] = -0.9
    meta[:stochastic] = false
    meta[:minhorizon] = 10
    meta[:maxhorizon] = ceil(Int, maxT / dt)
	meta[:discounted] = false
	meta[:episodes] = 400
	meta[:threshold] = -10.0

	render = (state,clearplot=false)->acrobotplot(state, params)

	m = SequentialProblem(S,X,A,p,d0,meta,render)
	return m
end

function rk_helper(s, tau, params::AcrobotParams)
	m1, m2, l1, l2, lc1, lc2, i1, i2, g = params.m1, params.m2, params.l1, params.l2, params.lc1, params.lc2, params.i1, params.i2, params.g

	d1 = m1*lc1^2 + m2*(l1^2 + lc2^2 + 2. * l1*lc2*cos(s[2])) + i1 + i2
	d2 = m2*(lc2^2 + l1*lc2*cos(s[2])) + i2

	phi2 = m2*lc2*g*cos(s[1] + s[2] - (π / 2.))
	phi1 = (-m2*l1*lc2*s[4]^2 * sin(s[2]) - 2. * m2*l1*lc2*s[4]*s[3] * sin(s[2]) + (m1*lc1 + m2*l1)*g*cos(s[1] - (π / 2.)) + phi2)

	newa2 = ((1. / (m2*lc2^2 + i2 - (d2^2) / d1)) * (tau + (d2 / d1)*phi1 - m2*l1*lc2*s[3]^2 * sin(s[2]) - phi2))
	newa1 = ((-1. / d1) * (d2*newa2 + phi1))
	return @SVector [s[3], s[4], newa1, newa2]
end

function acrobot_sim!(state::Array{T,1}, u::T, params::AcrobotParams{T}, dt) where {T}
	theta1, theta2, theta1Dot, theta2Dot = state
	hilf = @SVector [theta1, theta2, theta1Dot, theta2Dot] #deepcopy(state)

	h = dt / 10.0

	for i in 1:10
		s0_dot = rk_helper(hilf, u, params)
		s1 = hilf + (h / 2.) * s0_dot

		s1_dot = rk_helper(s1, u, params)
		s2 = hilf + (h / 2.) * s1_dot

		s2_dot = rk_helper(s2, u, params)
		s3 = hilf + (h / 2.) * s2_dot

		s3_dot = rk_helper(s3, u, params)
		hilf = hilf + (h / 6.) * (s0_dot + 2. * (s1_dot + s2_dot) + s3_dot)
	end

	theta1 = mod(hilf[1] + π, 2. * π) - π
	theta2 = mod(hilf[2] + π, 2. * π) - π

	theta1Dot = clamp(hilf[3], -4. * π, 4. * π)
	theta2Dot = clamp(hilf[4], -9. * π, 9. * π)

	state .= theta1, theta2, theta1Dot, theta2Dot

end

function get_torque(params::AcrobotParams, action::Int)
    if action <= 0 || action > 3
        error("Action needs to be an integer in [1, 3]")
    end
    u = 0.0
    u = (Float64(action) - 2.0) * params.fmag
    return u
end

function get_torque(params::AcrobotParams, action::Float64)
    u = clamp(action, -1., 1.) * params.fmag
    return u
end

function acrobot_sample_initial(stochastic_start)
    t = 0.0
    x = zeros(4)
    if stochastic_start
        x[1:2] .= rand(Uniform(-π * 5. / 180., π * 5. / 180.), 2)
    end
    return t, x
end
function acrobot_step!(state, action, params, dt, maxT, stochastic_start)
    u = get_torque(params, action)
    t, x = state
	acrobot_sim!(x, u, params, dt)
	t += dt

    reward = -0.1
    γ = 1.0
	done = acrobot_terminal(t, x, params, maxT)
	if done
        reward = 0.
        γ = 0.0
        t, x = acrobot_sample_initial(stochastic_start)
	end

	return t, x, reward, γ
end

function acrobot_terminal(t, state, params::AcrobotParams, maxT)
	elbowY = -params.l1*cos(state[1])
	handY = elbowY - params.l2*cos(state[1] + state[2])
	anglecond = handY > params.l1
	timecond = t ≥ maxT
    done = anglecond | timecond

	return done
end

@userplot AcrobotPlot
@recipe function f(ap::AcrobotPlot)#, state, params::AcrobotParams)
# @recipe function f(params::AcrobotParams, state)
	state, params = ap.args
	t,x = state
	theta1, theta2 = x[1:2]
	l1, l2 = params.l1, params.l2
	maxlen = (l1+l2)*1.2
	x1,y1 = 0., 0.
	x2 = x1 - sin(theta1)*l1
	y2 = y1 - cos(theta1)*l1
	x3 = x2 - sin(theta1 + theta2)*l2
	y3 = y2 - cos(theta1 + theta2)*l2

	legend := false
	xlims := (-maxlen, maxlen)
	ylims := (-maxlen, maxlen)
	grid := false
	ticks := nothing
	foreground_color := :white
	aspect_ratio := 1.

	# target line
	@series begin
		linecolor := :red
		linewidth := 5

		[0., 0.], [0., maxlen]
	end

	# arms
	@series begin
		linecolor := :black
		linewidth := 10

		[x1, x2, x3], [y1, y2, y3]
	end

end