############################### Perfect Incompressible Case ####################################
import getfem as gf
import numpy as np

########### Physical parameters #############
# For Rubber/tool
young = 0.7967
poisson = 0.4999
toolOffset = -0.4                      # Rubber Offset 0.4 mm
E1 = young                             # Young Modulus (N/mm^2)
nu1 = poisson                          # Poisson ratio
clambda1 = E1*nu1/((1+nu1)*(1-2*nu1))  # First Lame coefficient (N/mm^2)
cmu1 = E1/(2*(1+nu1))                  # Second Lame coefficient (N/mm^2)
C10 = 0.23                             # R1C10 - Mooney Rivlin 1st Parameter - 0.23 MPa = 0.23 N/mm^2
C01 = 0.057                            # R1C01 - Mooney Rivlin 2nd Parameter - 0.057 MPa = 0.057 N/mm^2

# For Workpiece
E2 = 1e15                              # Very high Young Modulus (N/mm^2) --> Close to rigid body
nu2 = 0.3
clambda2 = E2*nu2/((1+nu2)*(1-2*nu2)) # First Lame coefficient (N/mm^2)
cmu2 = E2/(2*(1+nu2))               # Second Lame coefficient (N/mm^2)

########### Numerical parameters #############
elements_degree = 3      # Degree of the finite element methods

######### Mesh Import ###############
file_msh1 = "tool_hemisphere.msh"
file_msh2 = "workpiece1.msh"
mesh1 = gf.Mesh('import','gmsh',file_msh1)
mesh1.set('optimize_structure')
mesh2 = gf.Mesh('import','gmsh',file_msh2)
mesh2.set('optimize_structure')

########## Boundary selection ###########
P1 = mesh1.pts()
P2 = mesh2.pts()
top = (P1[2,:] + 1e-6) > 0.0
workTop = (P2[2,:] + 1e-6) > -10.0 # Only top surface of the workpiece
work = (P2[2,:] + 1e-6) > -1001.0 ### Get the entire mesh ####
pidtop = np.compress(top,range(0,mesh1.nbpts()))
pidworkTop = np.compress(workTop,range(0,mesh2.nbpts()))
pidwork = np.compress(work,range(0,mesh2.nbpts()))

ftop = mesh1.faces_from_pid(pidtop) # Tool-hemisphere top surface
fcontactWork = mesh2.faces_from_pid(pidworkTop) # Workpiece's top face
fAllWork = mesh2.faces_from_pid(pidwork) # Entire Workpiece's
fcontact = mesh1.outer_faces_with_direction([0., 0., -1.], np.pi/4.5) # Contact boundary of the tool (rubber)
fbot = mesh2.outer_faces_with_direction([0., 0., -1.], 0.05) # Workpiece's bottom face - Constrained by ground below

TOOL_TOP_BOUND=1; CONTACT_BOUND=2; CONTACT_BOUND2=3; BOTTOM_BOUND=4; ALL_WORK=5;

########## Body - 1 ###############
# Definition of finite elements methods and integration method
mfu1 = gf.MeshFem(mesh1, 3)
mfu1.set_classical_fem(2)
pre_mflambda1 = gf.MeshFem(mesh1, 3)
pre_mflambda1.set_classical_fem(1)
# FEM for Stress Tensor
mfPKST = gf.MeshFem(mesh1)
mfPKST.set_classical_discontinuous_fem(2)
# Set few regions for the mesh
mesh1.set_region(TOOL_TOP_BOUND, ftop)
mesh1.set_region(CONTACT_BOUND, fcontact)
# Integration Methods
mim1 = gf.MeshIm(mesh1, 4)
mim1_contact = gf.MeshIm(mesh1, 4)


######## Body - 2 ##################
mfu2 = gf.MeshFem(mesh2, 3)
mfu2.set_classical_fem(2)
# Set few regions for the mesh
mesh2.set_region(CONTACT_BOUND2, fcontactWork)
mesh2.set_region(BOTTOM_BOUND, fbot)
mesh2.set_region(ALL_WORK, fAllWork)
# Integration Methods
mim2 = gf.MeshIm(mesh2, 4)
mim2_contact = gf.MeshIm(mesh2, 4)


######## Model definition ###############
md=gf.Model('real');

md.add_fem_variable('u1', mfu1)       # Displacement of the structure 1
md.add_filtered_fem_variable('lambda1', pre_mflambda1, CONTACT_BOUND)
# Add data of the model and add a elasticity brick for the rubber
md.add_initialized_data('cmu1', [cmu1])
md.add_initialized_data('clambda1', [clambda1])
md.add_initialized_data('mrParams', [0.23, 0.057])
# md.add_initialized_data('mrParams', [0.23])
md.add_finite_strain_elasticity_brick(mim1, 'Incompressible Mooney Rivlin', 'u1', '[0.23, 0.057]')
md.add_fem_variable('u2', mfu2)       # Displacement of the structure 2
# Add the Lame coefficients as data of the model and add a linearized elasticity brick for the workpiece
md.add_initialized_data('cmu2', [cmu2])
md.add_initialized_data('clambda2', [clambda2])
md.add_isotropic_linearized_elasticity_brick(mim2, 'u2', 'clambda2', 'cmu2')

# Large Sliding Contact condition
md.add_initialized_data('r', gamma0)
indbrick = md.add_integral_large_sliding_contact_brick_raytracing('r', 1.0)
md.add_slave_contact_boundary_to_large_sliding_contact_brick(indbrick, mim1_contact, CONTACT_BOUND, 'u1', 'lambda1')
md.add_master_contact_boundary_to_large_sliding_contact_brick(indbrick, mim2_contact, CONTACT_BOUND2, 'u2')
# md.add_rigid_obstacle_to_large_sliding_contact_brick(indbrick, 'z+6', 3)

# Add Incompressibility brick as we are using incompressible mooney-rivlin law
mfp = gf.MeshFem(mesh1,1)
mfp.set_classical_discontinuous_fem(1)
md.add_fem_variable('p', mfp)   # Hydrostatic pressure multiplier term
md.add_finite_strain_incompressibility_brick(mim1, 'u1', 'p')

# Add dirichlet displacement boundary conditions on the rubber surface (TOOL_TOP_BOUND) as there is no force condition
md.add_initialized_data('toolOffset', [0.0,0.0,toolOffset])
md.add_Dirichlet_condition_with_multipliers(mim1, 'u1', 1, TOOL_TOP_BOUND, 'toolOffset')

# Add dirichlet boundary conditions on the workpiece surface (BOTTOM_BOUND)
md.add_initialized_data('fixedBottom', [0.0,0.0,0.0])
md.add_Dirichlet_condition_with_multipliers(mim2, 'u2', 1, -1, 'fixedBottom') #  Entire Mesh
# md.add_Dirichlet_condition_with_multipliers(mim2, 'u2', 1, BOTTOM_BOUND, 'fixedBottom')

######### Model solve ##############
print 'Solve problem with ', md.nbdof(), ' dofs'
md.solve('max_res', 1E-7, 'max_iter', 20, 'noisy' , 'lsearch', 'simplest',  'alpha min', 0.8)


# Solution export
U1 = md.variable('u1')  # Values are correct
U2 = md.variable('u2')  # Values are correct
Pr = md.variable('p') # Hydrostatic pressure - Values are 10 to 50 times large and both positive/negative


####### Pressure and Stress Calculations  ##############


# Hydrostatic Pressure Calculations from Cauchy Stress (The values are zero)
pressureMethod1 = md.interpolation("-0.33*Trace(Cauchy_stress_from_PK2(Incompressible_Mooney_Rivlin_sigma(Grad_u1, [0.23, 0.057]),Grad_u1))", mfPKST, -1)

# Hydrostatic Pressure Calculations from Cauchy Stress after adding the incompressible pressure part (The values are 10 times higher and are both positive and negative)
pressureMethod2 = md.interpolation("-0.33*Trace(Cauchy_stress_from_PK2(Incompressible_Mooney_Rivlin_sigma(Grad_u1, [0.23, 0.057]),Grad_u1) - p*Id(3) )", mfPKST, -1)

slice = gf.Slice(('planar',0,[[0],[0],[-10+abs(toolOffset)]],[[0],[0],[1]]), mfu1, 3)
pressureOnSlice = gf.compute_interpolate_on(mfPKST, pressure, slice)
slice.export_to_pos('slice.pos', mfPKST,  pressure, 'Pressure(N/mm^2)', mfu1, U1, 'Displacement(mm)')