import Sofa
from Compliant import StructuralAPI, Tools

def createScene(rootNode):
    
    rootNode.gravity=[0,0,0]
    rootNode.createObject('RequiredPlugin', pluginName="Compliant")
    rootNode.createObject('RequiredPlugin', pluginName="ContactMapping")
    
    rootNode.createObject('BackgroundSetting', color='1 1 1')
#    rootNode.createObject('VisualStyle', displayFlags='showMechanicalMappings')

    rootNode.dt=1
    rootNode.gravity=[0,0,-0.1]
    
    rootNode.createObject('CompliantAttachButtonSetting', compliance=0.001)
    
    rootNode.createObject('CompliantImplicitSolver', name="odesolver", stabilization=True)
    rootNode.createObject('SequentialSolver', name="numsolver", iterations =10, precision=1e-14)
    rootNode.createObject('LDLTResponse', name="response")

    f = StructuralAPI.RigidBody( rootNode, "floor" )
    f.setManually(offset = [0,0,-2,0,0,0,1] )
    f.addCollisionMesh( 'mesh/square_xy.obj', scale3d=[1,1,1] )
    f.collision.addNormals()
    v=f.collision.addVisualModel()
    v.model.setColor(1,0,0,1)
    f.setFixed()

    r=[]
    r.append(addSphere(rootNode,[2,2,1],[0,0,0]))
    r.append(addSphere(rootNode,[2,1,1],[5,0,0]))
    r.append(addSphere(rootNode,[2,4,2],[1,0,3]))
    r.append(addSphere(rootNode,[1,2,2],[4,2,3]))
    r.append(addSphere(rootNode,[1,1,1],[2,1,8]))
    r.append(addSphere(rootNode,[1,1,1],[2,0,6]))
    r.append(addSphere(rootNode,[1,3,2],[0,-1,9]))
    r.append(addSphere(rootNode,[2,2,2],[0,-7,9]))
    r.append(addSphere(rootNode,[1,2,1],[0,-6,5]))
    r.append(addSphere(rootNode,[1,1,2],[2,-8,3]))

    for i,r1 in enumerate(r):
        addConstraint(r1,f,0.3)
        for j,r2 in enumerate(r):
#            if j>i:   # one way
            if j!=i:  # two ways
                addConstraint(r1,r2,0.1)

    return rootNode


nbObjects =0
nbDistances =0

def addSphere(node, scale3d=[1,1,1],position=[0,0,0]):
    global nbObjects
    r = StructuralAPI.RigidBody( node, "sphere"+str(nbObjects) )
    nbObjects+=1
    r.setFromMesh( 'mesh/sphere.obj', density=1E-3,scale3d=scale3d,offset = [position[0],position[1],position[2],0,0,0,1] )
    r.addCollisionMesh( 'mesh/sphere.obj', scale3d=scale3d )
    r.collision.addVisualModel()
    r.collision.addNormals()
    return r

def addConstraint(r1,r2,offsetBV):
    global nbDistances
    distance = r1.collision.node.createChild("distance"+str(nbDistances) )
    nbDistances+=1
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
    
    distance.createObject('ClosestPointEngine', name="cp", sourcePosition="@"+r1.collision.node.getPathName()+"/topology.position", sourceRigidPosition="@"+r1.node.getPathName()+"/dofs.position", targetPosition="@"+r2.collision.node.getPathName()+"/topology.position", targetRigidPosition="@"+r2.node.getPathName()+"/dofs.position", printLog=False,drawMode="0",method="1",useBV=True, offsetBV=offsetBV,cacheSize=5,parallel=True)
    distance.createObject('ProjectionToVertexPlaneEngine', name="pm", printLog=True, template="Vec3",
                            sourcePosition=r1.collision.dofs.getLinkPath()+".position", targetPosition=r2.collision.dofs.getLinkPath()+".position",
                            indexPairs="@cp.indexPairs" ,
                            normals2="@"+r2.collision.node.getPathName()+"/normalsFromPoints.normals",
                            useRestLength=True, restLength=0 )
    # distance.createObject('ProjectionToMeshEngine', name="pm", printLog=True,
    #                         sourcePosition=r1.collision.dofs.getLinkPath()+".position", targetPosition=r2.collision.dofs.getLinkPath()+".position",
    #                         indexPairs="@cp.indexPairs" ,
    #                         triangles2="@"+r2.collision.node.getPathName()+"/topology.triangles", quads2="@"+r2.collision.node.getPathName()+"/topology.quads",
    #                         useRestLength=True, restLength=0 )
    distance.createObject('DistanceToMeshMultiMapping', template="Vec3d,Vec1d", name="mapping", input="@"+r1.collision.node.getPathName()+" @"+r2.collision.node.getPathName(), output="@.", indexPairs="@cp.indexPairs", closests="@pm.output", printLog=False, bilateral=False, keepNSmallest="10")


#    distance.createObject('PenetrationForceField', hardStiffness="1", separatingStiffness="0.01", proximity="0.01",damping="0",isCompliance='0')

    distance.createObject('UniformCompliance', compliance=1E-5,isCompliance='1',resizable=True)
    distance.createObject('UnilateralConstraint')
    r2.collision.node.addChild(distance)



