import Sofa

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

    rootNode.dt=0.03
    rootNode.gravity=[0,-1,-10]

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

#    rootNode.createObject('EulerImplicit',rayleighStiffness='0',rayleighMass='0')
#    rootNode.createObject('CGLinearSolver', iterations=250, tolerance=1e-14, threshold=1e-14)

    sphere = rootNode.createChild("sphere")
    sphere.createObject('MeshObjLoader', name='loader', filename='mesh/sphere.obj', triangulate='1', translation='0 0 -3', scale3d='2 2.5 3')
    sphere.createObject('MechanicalObject', template="Vec3d", name="dof", position="@loader.position")
    sphere.createObject('NormalsFromPoints', name="NormalsFromPoints", template="Vec3d", position="@dof.position", triangles="@loader.triangles", quads="@loader.quads")
    sphere.createObject('UniformMass', name="Mass", totalmass="1")
    sphere.createObject('FixedConstraint', fixAll="True")
    sphereVisu = sphere.createChild("visu")
    sphereVisu.createObject('VisualModel', filename='mesh/sphere.obj')
    sphereVisu.createObject('IdentityMapping')
    
    cube = rootNode.createChild("green_cube_projectionToMesh")
    cube.createObject('RegularGrid', name="RegularGrid", nx=5, ny=5, nz=5, xmin=-0.5, xmax=0.5, ymin=-0.5, ymax=0.5, zmin=0.2, zmax=1.2)
    cube.createObject('MechanicalObject', template="Vec3d", name="dof", position="@RegularGrid.position")
    cube.createObject('UniformMass', name="Mass", totalmass=1)
#    cube.createObject('UniformVelocityDampingForceField',dampingCoefficient='0.1')
    cube.createObject('HexahedronFEMForceField', name="FEM", youngModulus=1000, poissonRatio=0)
    cube.createObject('BoxROI', template="Vec3d", name="contactRoi", box="-10 -10 -2 10 10 0.201", drawBoxes=True)
    cubeVisu = cube.createChild("visu")
    cubeVisu.createObject('VisualModel', color="green")
    cubeVisu.createObject('IdentityMapping')

#   Two ways :
#    distance = cube.createChild("distance_cubetospheree")
#    distance.createObject('ClosestPointEngine', name="cp", sourcePosition="@../dof.position", targetPosition="@/sphere/dof.position", sourceROI="@../contactRoi.indices",  printLog=False,drawMode="0")
#    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
#    distance.createObject('DistanceToMeshMultiMapping', template="Vec3d,Vec1d", name="mapping", input="@.. @/sphere/.", output="@.", indexPairs="@cp.indexPairs", triangles2="@/sphere/loader.triangles", printLog=False)
#    distance.createObject('UniformCompliance', compliance=1e-5,isCompliance='0')
##    distance.createObject('UnilateralConstraint')
##    distance.createObject('ConstraintValue')
##    distance.createObject('Stabilization')
#    sphere.addChild(distance)

#   One way :
    distance = cube.createChild("distance_cubetospheree")
    distance.createObject('ClosestPointEngine', name="cp", sourcePosition="@../dof.position", targetPosition="@/sphere/loader.position", sourceROI="@../contactRoi.indices",  printLog=False,drawMode="0")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
    distance.createObject('ProjectionToMeshEngine', name="pm", printLog=True,
                        sourcePosition="@../dof.position", targetPosition="@/sphere/loader.position",
                        indexPairs="@cp.indexPairs" ,
                        triangles="@/sphere/loader.triangles",
                        useRestLength=True )
    distance.createObject('ClosestKeepNSmallestEngine', name="kn", printLog=True,
                    closests="@pm.output",
                    indexPairs="@cp.indexPairs",
                    keepNSmallest=5,
                    bilateral=False )
    distance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping", indexPairs="@cp.indexPairs", validPairs="@kn.output", position="@/sphere/loader.position", closests="@pm.output", printLog=False)
    distance.createObject('UniformCompliance', compliance=1e-5,isCompliance='0')




    cube = rootNode.createChild("blue_cube_projectionToTangentPlanes")
    cube.createObject('RegularGrid', name="RegularGrid", nx=5, ny=5, nz=5, xmin=-0.5, xmax=0.5, ymin=-0.5, ymax=0.5, zmin=0.2, zmax=1.2)
    cube.createObject('MechanicalObject', template="Vec3d", name="dof", position="@RegularGrid.position")
    cube.createObject('UniformMass', name="Mass", totalmass=1)
#    cube.createObject('UniformVelocityDampingForceField',dampingCoefficient='0.1')
    cube.createObject('HexahedronFEMForceField', name="FEM", youngModulus=1000, poissonRatio=0)
    cube.createObject('BoxROI', template="Vec3d", name="contactRoi", box="-10 -10 -2 10 10 0.201", drawBoxes=True)
    cubeVisu = cube.createChild("visu")
    cubeVisu.createObject('VisualModel', color="blue")
    cubeVisu.createObject('IdentityMapping')

#   One way :
    distance = cube.createChild("distance_cubetospheree")
    distance.createObject('ClosestPointEngine', name="cp", sourcePosition="@../dof.position", targetPosition="@/sphere/loader.position", sourceROI="@../contactRoi.indices",  printLog=False,drawMode="0")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
    distance.createObject('ProjectionToVertexPlaneEngine', name="pm", printLog=True,
                    sourcePosition="@../dof.position", targetPosition="@/sphere/loader.position",
                    indexPairs="@cp.indexPairs" ,
                    normal="@/sphere/NormalsFromPoints.normals",
                    useRestLength=True )
    distance.createObject('ClosestKeepNSmallestEngine', name="kn", printLog=True,
                    closests="@pm.output",
                    indexPairs="@cp.indexPairs",
                    keepNSmallest=5,
                    bilateral=False)
    distance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping", indexPairs="@cp.indexPairs", validPairs="@kn.output", position="@/sphere/loader.position", closests="@pm.output", printLog=False)
    distance.createObject('UniformCompliance', compliance=1e-5,isCompliance='0')


    return rootNode
