import Sofa
import SofaPython.Tools

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,0,-9]

    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('LUSolver', name="numsolver")
    rootNode.createObject('MinresSolver', name="numsolver")
#     rootNode.createObject('LDLTSolver', name="numsolver")

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

    plane = rootNode.createChild("plane")
    plane.createObject('RegularGrid', nx=10, ny=10, nz=1, xmin=-10, xmax=10, ymin=-10, ymax=10, zmin=0, zmax=0)
#    triangles = plane.createChild("triangles")
#    triangles.createObject('TriangleSetTopologyContainer', name="Container")
#    triangles.createObject('TriangleSetTopologyModifier', name="Topology, Modifier")
#    triangles.createObject('Quad2TriangleTopologicalMapping')
    plane.createObject('MechanicalObject', template="Vec3d", name="dof", position="@RegularGrid.position")
    plane.createObject('NormalsFromPoints', name="NormalsFromPoints", template="Vec3d", position="@dof.position", triangles="@RegularGrid.triangles", quads="@RegularGrid.quads")
    plane.createObject('UniformMass', name="Mass", totalmass="1")
    plane.createObject('FixedConstraint', fixAll="True")
    planeVisu = plane.createChild("visu")
    planeVisu.createObject('OglModel')
    planeVisu.createObject('IdentityMapping')
    
    cube = rootNode.createChild("cube")
    cubeTopology = cube.createObject('RegularGrid', name="topology", nx=5, ny=5, nz=5, xmin=-4, xmax=4, ymin=-4, ymax=4, zmin=4, zmax=8)
    cubeDofs = cube.createObject('MechanicalObject', template="Vec3d", name="dof", position="@topology.position")
    cube.createObject('BoxROI', template="Vec3d", name="contactRoi", box="-0.6 -0.6 3.9 0.6 0.6 4.5", drawBoxes=True, position="@dof.position")
    cube.createObject('UniformMass', name="Mass", totalmass=1)
    cube.createObject('HexahedronFEMForceField', name="FEM", youngModulus=100, poissonRatio=0)
    cube.createObject('UniformVelocityDampingForceField',dampingCoefficient='0.01')

    distance = cube.createChild("distance")
    distance.createObject('ClosestPointEngine', name="cp", sourcePosition="@/cube/dof.position", targetPosition="@/plane/RegularGrid.position", sourceROI="@/cube/contactRoi.indices",  printLog=False,drawMode="0")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
    distance.createObject('PenetrationForceField', name="pff", hardStiffness="3", separatingStiffness=".3", proximity=".3",damping="0",isCompliance='0',computeColors="True")


    ###### SET A REST LENGTH BASED ON X-POS
    cubeTopology.init()
    restLengths = [ x[0]*.2+2 for x in cubeTopology.position]

    distance.createObject('ProjectionToVertexPlaneEngine', name="pm", printLog=True,
                    sourcePosition="@/cube/dof.position", targetPosition="@/plane/RegularGrid.position",
                    indexPairs="@cp.indexPairs" ,
                    normals2="@plane/NormalsFromPoints.normals",
                    useRestLength=True, restLength=SofaPython.Tools.listToStr(restLengths) )

    distance.createObject('DistanceToMeshMultiMapping', template="Vec3d,Vec1d", name="mapping", input="@/cube/. @/plane/.", output="@.", indexPairs="@cp.indexPairs", closests="@pm.output", printLog=False, showColor="@pff.colors", showObjectScale="0.01")
#    distance.createObject('UniformCompliance', compliance=1E-10,isCompliance='1')
#    distance.createObject('UnilateralConstraint')
#    distance.createObject('ConstraintValue')
#    distance.createObject('Stabilization')
    plane.addChild(distance)

    cubeVisu = cube.createChild("visu")
    cubeVisu.createObject('OglModel', color="green")
    cubeVisu.createObject('IdentityMapping')
    
    return rootNode
