# encoding: utf-8
# 2019 © Vasileios Angelidakis <v.angelidakis2@ncl.ac.uk>
# A script to check the block generation algorithm, which subdivides Potential Blocks by a set of predefined discontinuities, creating zero-porosity assemblies of polyhedral particles.

# The BlockGen engine uses the following algorithms:
# CW Boon, GT Houlsby, S Utili (2012).  A new algorithm for contact detection between convex polygonal and polyhedral particles in the discrete element method.  Computers and Geotechnics 44, 73-82. 
# CW Boon, GT Houlsby, S Utili (2015).  A new rock slicing method based on linear programming.  Computers and Geotechnics 65, 12-29. 

if ('PotentialBlocks' in features):

	errors=0
	errMsg=""

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Set up BlockGen parameters
	p=BlockGen()
	p.maxRatio = 1000.0;
	p.minSize = 7.0;
	p.density = 2650 #kg/m3
	p.dampingMomentum = 0.0
	p.viscousDamping = 0.0
	p.Kn = 2.0e9  	#2 GPa/m
	p.Ks = 0.1e9 	#0.1 GPa/m
	p.frictionDeg = 50.0 #degrees
	p.traceEnergy = False
	p.defaultDt = 1e-4
	p.rForPP = 0.04 #0.04 #0.4
	p.kForPP = 0.0
	p.RForPP = 1800.0 #1800.0
	p.gravity = [0,0,9.81]
	#p.inertiaFactor = 1.0
	#p.initialOverlap = 1e-6
	p.exactRotation = True
	p.shrinkFactor = 1.0
	p.boundarySizeXmin = 20.0; #South
	p.boundarySizeXmax = 20.0; #North
	p.boundarySizeYmin = 20.0; #West
	p.boundarySizeYmax = 20.0; #East
	p.boundarySizeZmin =  0.0; #Up
	p.boundarySizeZmax = 40.0; #Down
	p.persistentPlanes = False
	p.jointProbabilistic = True
	p.opening = False
	p.boundaries = True
	p.slopeFace = False
	p.twoDimension = False
#	p.unitWidth2D = 9.0
#	p.calJointLength = False
	p.intactRockDegradation = True
	#p.useFaceProperties = False
	p.neverErase = False # Must be used when tension is on
	p.sliceBoundaries = False
	p.filenamePersistentPlanes = ''
	p.filenameProbabilistic = checksPath+'/data/joints.csv'
	p.filenameOpening = '' #'./joints/opening.csv'
	p.filenameBoundaries = ''
	p.filenameSlopeFace =''
	p.filenameSliceBoundaries = ''
	p.directionA=Vector3(1,0,0)
	p.directionB=Vector3(0,1,0)
	p.directionC=Vector3(0,0,1)

	p.saveBlockGenData=True
	p.outputFile=""

	p.load()
	O.engines[2].lawDispatcher.functors[0].initialOverlapDistance = 0 # = p.initialOverlap - 1e-6
	O.engines[2].lawDispatcher.functors[0].allowBreakage = False

	O.engines[2].physDispatcher.functors[0].kn_i = 2.0e9
	O.engines[2].physDispatcher.functors[0].ks_i = 0.1e9

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Checks on the generated rockmass
	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check number of generated blocks
	if len(O.bodies)!=6: #6 bodies
		errMsg+=str( "Wrong number of generated blocks: " + str(len(O.bodies))+" , instead of: "+ str(6) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check number of vertices of all the generated blocks
	sumVertices = sum( len(b.shape.vertices) for b in O.bodies ) #38 vertices in total
	if sumVertices!=38:
		errMsg+=str( "Wrong number of vertices of all the generated blocks: " + str(sumVertices)+" , instead of: "+ str(38) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check number of faces of all the generated blocks
	a = sum(len(b.shape.a) for b in O.bodies )
	b = sum(len(b.shape.b) for b in O.bodies )
	c = sum(len(b.shape.c) for b in O.bodies )
	d = sum(len(b.shape.d) for b in O.bodies )

	average=(a+b+c+d)/4.
	if a!=average or b!=average or c!=average or c!=average or a!=32: #32 faces of all bodies
		errMsg+=str( "Wrong calculation of faces of all the generated blocks: " + str(average)+" , instead of: "+ str(32) +"\n" ) #The planes should have the same size of a,b,c,d vectors
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check volume of the rockmass
	tol=1e-3
	sumVolume = sum( b.shape.volume for b in O.bodies )
	target = (p.boundarySizeXmin + p.boundarySizeXmax) * (p.boundarySizeYmin + p.boundarySizeYmax) * (p.boundarySizeZmin + p.boundarySizeZmax); V=target
	if (abs(sumVolume-target)/abs(target)>tol):
		errMsg+=str( "Wrong volume of rockmass: " + str(sumVolume)+" , instead of: "+ str(target) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check mass of the rockmass
	tol=1e-3
	sumMass = sum (b.state.mass for b in O.bodies)
	target = V*p.density
	if (abs(sumMass-target)/abs(target)>tol):
		errMsg+=str( "Wrong mass of rockmass: " + str(sumMass)+" , instead of: "+ str(target) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check centroid of the generated rockmass
	tol=1e-3
	target=Vector3.Zero
	target = Vector3( 0.5*(p.boundarySizeXmax-p.boundarySizeXmin), 0.5*(p.boundarySizeYmax-p.boundarySizeYmin), 0.5*(p.boundarySizeZmax-p.boundarySizeZmin) ); centroid=target;
	x = sum( b.state.pos[0]*b.shape.volume for b in O.bodies )/sumVolume
	y = sum( b.state.pos[1]*b.shape.volume for b in O.bodies )/sumVolume
	z = sum( b.state.pos[2]*b.shape.volume for b in O.bodies )/sumVolume

	if (abs( x - target[0] )/abs( 0.5*(p.boundarySizeXmax + p.boundarySizeXmin) ) > tol  or 
	    abs( y - target[1] )/abs( 0.5*(p.boundarySizeYmax + p.boundarySizeYmin) ) > tol  or
	    abs( z - target[2] )/abs( 0.5*(p.boundarySizeZmax + p.boundarySizeZmin) ) > tol  ):
		errMsg+=str( "Wrong centroid of rockmass: " + str(Vector3(x,y,z))+" , instead of: "+ str(target) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	# Check inertia of the generated rockmass
	tol=1e-3
	target=Vector3.Zero
	target[0] = 1/12. * V * ( (p.boundarySizeYmax + p.boundarySizeYmin)**2 + (p.boundarySizeZmax + p.boundarySizeZmin)**2 )
	target[1] = 1/12. * V * ( (p.boundarySizeXmax + p.boundarySizeXmin)**2 + (p.boundarySizeZmax + p.boundarySizeZmin)**2 )
	target[2] = 1/12. * V * ( (p.boundarySizeXmax + p.boundarySizeXmin)**2 + (p.boundarySizeYmax + p.boundarySizeYmin)**2 )

	inertiaGlobal=Matrix3.Zero
	for b in O.bodies:
		inertiaDiagonal =  b.state.ori.toRotationMatrix() * b.shape.inertia.asDiagonal() * b.state.ori.conjugate().toRotationMatrix()

		relPos = (b.state.pos - centroid)
		dx = relPos[0]
		dy = relPos[1]
		dz = relPos[2]

		Ixx = inertiaDiagonal[0][0] + b.shape.volume * ( dy**2 + dz**2 )
		Iyy = inertiaDiagonal[1][1] + b.shape.volume * ( dx**2 + dz**2 )
		Izz = inertiaDiagonal[2][2] + b.shape.volume * ( dx**2 + dy**2 )
		Ixy = inertiaDiagonal[0][1] - b.shape.volume * ( dx*dy )
		Ixz = inertiaDiagonal[0][2] - b.shape.volume * ( dx*dz )
		Iyz = inertiaDiagonal[1][2] - b.shape.volume * ( dy*dz )

		inertiaDiagonal = Matrix3(Ixx, Ixy, Ixz, Ixy, Iyy, Iyz, Ixz, Iyz, Izz)
		inertiaGlobal = inertiaGlobal + inertiaDiagonal

	if (abs( inertiaGlobal[0][0] - target[0])/abs(target[0]) > tol  or 
	    abs( inertiaGlobal[1][1] - target[1])/abs(target[1]) > tol  or
	    abs( inertiaGlobal[2][2] - target[2])/abs(target[2]) > tol  or

	  ( abs( inertiaGlobal[0][1] ) + abs( inertiaGlobal[1][0] ) + 
	    abs( inertiaGlobal[0][2] ) + abs( inertiaGlobal[2][0] ) + 
	    abs( inertiaGlobal[1][2] ) + abs( inertiaGlobal[2][1] ) 
	  )/abs( inertiaGlobal.trace() ) > tol ):
		errMsg+=str( "Wrong inertia of rockmass: " + str(inertiaGlobal)+" , instead of: "+ str(target.asDiagonal()) +"\n" )
		errors += 1

	# ----------------------------------------------------------------------------------------------------------------------------------------------- #
	if (errors):
		raise YadeCheckError(errMsg)	#Test has failed

else:
	print("skip BlockGen check, PotentialBlocks not available")

