########################################## encoding: utf-8 #######################################
#-*- coding: utf-8 -*-
from yade import plot,qt
from yade.gridpfacet import *
import math

O.dt = 0.0000001

Rad = 0.1
Factor=1.5
m_viscosity=3
m_roughness=0.02

###############################################################################################
############################################# Mask ############################################
###############################################################################################
# mask avec un 1 en commun se voient

# 1 -> 0b001 -> Reste - Manilles
# 2 -> 0b010 ->
# 3 -> 0b011 -> Manilles et cable - facets bloc
# 4 -> 0b100 ->
# 5 -> 0b101 ->
# 6 -> 0b110 ->


# 4 -> 0b100 -> Avoidintaraction -> les objets dans les masks qui ont un 1 en commun ne se voient pas à l'intérieur du mask

MaskCyl = 3
MaskCollider = 2

E1_1 = 500e6
E2_1 = 1460e6
Ef_1 = 5869.565e6
Defini_1 = 0.0001
Def1_1 = 0.060
Def2_1 = 0.015+Def1_1
Deff_1 = 0.041+Def2_1
SSVal_Simple=[(Defini_1,2*Defini_1*Ef_1),(Def1_1,Def1_1*E1_1),(Def2_1,Def2_1*E2_1),(Deff_1,2*Deff_1*Ef_1)]#[(0.0001,5.09e6),(0.030,50e6),(0.043,155e6),(0.067,1250e6)]

O.materials.append( WireMat( young=5e9,poisson=0.3,frictionAngle=radians(10),density=7850,diameter=2*0.006,isDoubleTwist=False,label='gridNodeNetPackSimple',strainStressValues=SSVal_Simple) ) # Material to create the GridNode

O.materials.append(
        FrictMat(
            density=2600,
            frictionAngle=math.radians(35.),
            poisson=0.5,
            young=1e8*Rad,
            label='Frict'
        ))
diameterPart=Rad*2
young=1e8
poisson=0.3
kn=young*(diameterPart/2)
ks=kn*poisson
en=0.1      # Restitution coeff in normal direction
es=1.0      # Restitution coeff in tangential direction
damp=0.00
sphFricAng=40
sphVol=4*(math.pi/3)*pow((diameterPart/2),3)
cn=sqrt((pow(math.log(en),2)*2*kn*sphVol*2000)/(pow(math.pi,2)+pow(math.log(en),2)))
cs=0


material='Frict'
#S1 = O.bodies.append(sphere(center=[0,0,0],radius=Rad,material=material,fixed=True))
S2 = O.bodies.append(sphere(center=[0,0,Factor*2.1*Rad],radius=Rad,material=material,fixed=False))
Stest = O.bodies.append(sphere(center=[0,0,3*Factor*2.1*Rad],radius=Rad,material=material,fixed=False))
#S3 = O.bodies.append(sphere(center=[0,((Factor+0.05)*2*Rad),0],radius=Rad,material=material,fixed=False))
#S4 = O.bodies.append(sphere(center=[0,0,-((Factor+0.05)*2*Rad)],radius=Rad,material=material,fixed=False))
#W1 = O.bodies.append(aabbWalls([(-1,-1,0),(1,1,0)],thickness=0.1,material=material,oversizeFactor=1.0))
#for i in W1[:-1]:
#        O.bodies.erase(i)
G1 = O.bodies.append(gridNode([0,0,0],radius = Rad/4.,material='gridNodeNetPackSimple',fixed=True))
G2 = O.bodies.append(gridNode([+3*Rad,0,0],radius = Rad/4.,material='gridNodeNetPackSimple',fixed=True))
G3 = O.bodies.append(gridNode([+1.5*Rad,-1.5*Rad,0],radius = Rad/4.,material='gridNodeNetPackSimple',fixed=True))
C1 = O.bodies.append(gridConnection(G1,G2,radius = Rad/4.,material=material, mask = MaskCyl))
C2 = O.bodies.append(gridConnection(G2,G3,radius = Rad/4.,material=material, mask = MaskCyl))
C3 = O.bodies.append(gridConnection(G1,G3,radius = Rad/4.,material=material, mask = MaskCyl))
#O.bodies[S1].state.blockedDOFs='xyzXYZ'
#O.bodies[S2].state.blockedDOFs='xyzXYZ'

O.bodies[S2].state.vel[2]=-3.
O.bodies[Stest].state.vel[2]=-10.
#O.bodies[S3].state.vel[1]=-5.
#O.bodies[S4].state.vel[2]=5.

#O.bodies[S2].state.blockedDOFs='Z'


theta_method= 0.55  #1 : Euler Backward, 0.5 : trapezoidal rule, 0 : not used, 0.55 : suggested optimum
resolution=1        #Change normal component resolution method, 0: Iterative exact resolution (theta method, linear contact), 1: Newton-Rafson dimentionless resolution (theta method, linear contact), 2: Newton-Rafson with nonlinear surface deflection (Hertzian-like contact)
O.engines=[
    ForceResetter(),
    InsertionSortCollider([Bo1_Sphere_Aabb(aabbEnlargeFactor=Factor, label="aabb"),Bo1_Wall_Aabb(),Bo1_Box_Aabb(),Bo1_GridConnection_Aabb()],verletDist=-0.1,allowBiggerThanPeriod=False),
    InteractionLoop(
        [Ig2_Sphere_Sphere_ScGeom6D(interactionDetectionFactor=Factor,label="Ig2"),
         Ig2_Box_Sphere_ScGeom6D(),
         Ig2_GridNode_GridNode_GridNodeGeom6D(),
         Ig2_Sphere_GridConnection_ScGridCoGeom()],

        [Ip2_FrictMat_FrictMat_FrictPhys(),
         Ip2_WireMat_WireMat_WirePhys(),
        Ip2_FrictMat_FrictMat_LubricationPhys(eta=m_viscosity,eps=m_roughness),

        ],

        [Law2_ScGeom_ImplicitLubricationPhys(activateNormalLubrication=True,
                                             activateTangencialLubrication=True,
                                             activateTwistLubrication=False,
                                             activateRollLubrication=False,
                                             theta=theta_method,
                                             resolution=resolution,
                                             warnedOnce=True,
                                             maxSubSteps=20,
                                             debug=False,),
            Law2_ScGeom_WirePhys_WirePM(),
         Law2_ScGeom_FrictPhys_CundallStrack(),
        ]
    ),
    NewtonIntegrator(damping=0.,gravity=[0,0,-9.81]),
    PyRunner(command='addPlotData()',iterPeriod=100),
#   PyRunner(command='maf()',iterPeriod=100),
    #PyRunner(command='print(maxFnL, O.bodies[S2].state.vel[2])',iterPeriod=100000)
]
collider.avoidSelfInteractionMask = MaskCollider
#def maf():
#   if (O.bodies[S2].state.pos[2]-O.bodies[S1].state.pos[2])<2*Rad+0.02:
#       O.bodies[S2].state.vel[2]=0.
#       O.bodies[S2].state.blockedDOFs='xyzXYZ'

#maxFnL=0.
#Ekin=0.5*O.bodies[S2].state.mass*O.bodies[S2].state.vel.norm()**2
#Epot=O.bodies[S2].state.mass*9.81*(O.bodies[S2].state.pos[2]-O.bodies[S1].state.pos[2])
#Etot=Ekin+Epot
def addPlotData():
    global maxFnL,Etot,Ekin,Epot
    try:
        Fnc = O.interactions[0,1].phys.normalContactForce[2]
        FnL = O.interactions[0,1].phys.normalLubricationForce[2]
        maxFnL=max(maxFnL,FnL)
    except:
        Fnc=0
        FnL=0
    try:
        O.interactions[0,1].isActive
        active=1
    except:
        active=0
    Ekin=0.5*O.bodies[S2].state.mass*O.bodies[S2].state.vel.norm()**2
    Epot=O.bodies[S2].state.mass*9.81*(O.bodies[S2].state.pos[2]-O.bodies[Stest].state.pos[2])
    Etot=Ekin+Epot
    if (O.bodies[S2].state.pos[2]-O.bodies[Stest].state.pos[2]-2*Rad)<0:
        pene=O.bodies[S2].state.pos[2]-O.bodies[Stest].state.pos[2]-2*Rad
    else:
        pene=0


    delta = (O.bodies[S2].state.pos[2]-O.bodies[Stest].state.pos[2])-(2*Factor*Rad)
    deltaDot = O.bodies[S2].state.vel[2] - O.bodies[Stest].state.vel[2]
    plot.addData(delta=delta, time1=O.time, time2=O.time, time3=O.time, time4=O.time,Fnc = Fnc,FnL = FnL,deltaDot = deltaDot,active=active,Ekin=Ekin,Epot=Epot,Etot=Etot,pene=pene)

addPlotData()
plot.plots={'time1':('delta',None,'active'), 'time2':('deltaDot'), 'time3':('Fnc',None,('FnL','g')), 'time4':('pene')}
#plot.plots={'time4':('Fnc',None,('FnL','g')),'time1':'active'};
plot.plot(subPlots=True)
def stopMe():
    if O.iter>10000 and O.bodies[S2].state.pos[2]>((Factor+0.05)*2*Rad):
        O.pause()
        plot.saveGnuplot('graphs/'+O.bodies[S1].material.label+'Lubrication_visco'+str(m_viscosity)+'_rought'+str(m_roughness),term='png',extension='png')

qt.View()