import Sofa

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

    rootNode.createObject('CompliantAttachButtonSetting', compliance=0.001)
    
    rootNode.createObject('CompliantImplicitSolver', name="odesolver", stabilization=True)
#    rootNode.createObject('SequentialSolver', name="numsolver", iterations =250, precision=1e-14)
    rootNode.createObject('LDLTSolver', name="numsolver")

    mesh = rootNode.createChild("mesh")
        
    mesh.createObject('MeshObjLoader', name="loader", filename="mesh/snake_body.obj" )
    mesh.createObject('MeshTopology', name="topology", src="@loader" )
    
    mesh.createObject('MechanicalObject', template="Vec3d", name="dof", showObject="True")
    mesh.createObject('UniformMass', name="Mass", totalmass="1")
    mesh.createObject('FixedConstraint', fixAll="True")

    meshVisu = mesh.createChild("visu")
    meshVisu.createObject('OglModel', color="red")
    meshVisu.createObject('IdentityMapping')

    point = rootNode.createChild("point")
    point.createObject('MechanicalObject', template="Vec3d", name="dof", position="2 2 2", velocity="0 0 0", showObject=True, showObjectScale=0.5, drawMode=1)
    point.createObject('UniformMass', name="Mass", totalmass=1)
    
    point.createObject('ClosestPointEngine', name="cp", sourcePosition="@point/dof.position", targetPosition="@mesh/topology.position", printLog=False, drawMode="1",method="1",cacheSize=50)
    point.createObject('ProjectionToMeshEngine', name="pm",
                        sourcePosition="@point/dof.position",
                        targetPosition="@mesh/topology.position",
                        indexPairs="@cp.indexPairs" ,
                        triangles2="@mesh/loader.triangles", quads2="@mesh/loader.quads" )

    projection = point.createChild("projection")
    projection.createObject('MechanicalObject', template="Vec3d", name="dof", showObject="True", showObjectScale=0.2, drawMode=2 )
    projection.createObject('ProjectionToMeshMultiMapping', template="Vec3d,Vec3d", name="mapping", input="@point @mesh", output="@.", indexPairs="@../cp.indexPairs" , closests="@../pm.output", printLog=True, showObjectScale=".2", showColor="0 1 0 0.5",constant=True )
    mesh.addChild(projection)

    distance = projection.createChild("distance")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof" )
    distance.createObject('EdgeSetTopologyContainer', edges="0 1" )
    distance.createObject('DistanceMultiMapping', template="Vec3d,Vec1d", name="mapping", input="@/point/projection/. @/point/.", output="@.", indexPairs="0 0  1 0", showObjectScale="0.01" )
    distance.createObject('UniformCompliance', compliance=0, isCompliance=1)
    #distance.createObject('Stabilization')
    mesh.addChild(distance)

    
    distancedirect = point.createChild("distancemapping")
    distancedirect.createObject('MechanicalObject', template="Vec1d", name="dof")
    distancedirect.createObject('DistanceToMeshMultiMapping',template="Vec3d,Vec1d",name="mapping", input="@point @mesh", output="@.", indexPairs="@../cp.indexPairs" , closests="@../pm.output", printLog=True)
#    distancedirect.createObject('UniformCompliance', compliance=1E-5,isCompliance='1',resizable=True)

    mesh.addChild(distancedirect)

    
    return rootNode
