import Sofa
import SofaPython.Tools
import random

def createSceneAndController(rootNode):
    rootNode.createObject("RequiredPlugin", pluginName="Flexible")
    rootNode.createObject("RequiredPlugin", pluginName="Image")
    rootNode.createObject("RequiredPlugin", pluginName="LinearSubspace")

#    rootNode.createObject("MeshObjLoader", name="loader", filename="mesh/Armadillo_simplified.obj")
    rootNode.createObject("MeshObjLoader", name="loader", filename="/home/ulysse/Documents/temp/sphere_big.obj")

    meshVisualNode = rootNode.createChild("meshVisual")
    meshVisualNode.createObject("VisualModel", src="@../loader", color="0.75 0.75 0.75 0.5")

    rasterNode = rootNode.createChild("raster")
    rasterNode.createObject("MeshToImageEngine", template="ImageUC", name="image", src="@../loader", voxelSize=0.5, value="1", fillInside=True, insideValue="2")

    imageVisualNode = rootNode.createChild("imageVisual")
    imageVisualNode.createObject("ImageViewer", template="ImageUC",  name="viewer", src="@/raster/image")

    hexaNode = rootNode.createChild("hexa")
    hexaNode.createObject("ImageSampler", template="ImageUC",  name="sampler", src="@/raster/image")

    tetraNode = rootNode.createChild("tetra")
    tetraNode.createObject("Hexa2TetraEngine", name="converter", hexaIndex="@/hexa/sampler.hexahedra" )
    tetraNode.createObject("MeshTopology", name="topology", position="@/hexa/sampler.position", tetrahedra="@converter.tetraIndex", drawTetrahedra=True)

    tetraNode.createObject("VTKExporter",filename="/home/ulysse/Documents/temp/exported.vtu", position="@/hexa/sampler.position", tetras=True, edges=False, exportAtBegin=True)

#   ^
#  /!\ Warning : the resulting tetrahedral mesh is not always manifold (in case of isolated voxel connected to the rest through a vertex or an edge).
#  ___


#    return

    lsNode = tetraNode.createChild("ls")

    handle_nodes = [1, 2, 3, 4]
    handle_regions = [[]]
#    handle_regions = [[100, 101, 102, 103, 104, 105],[200, 201, 202, 203, 204, 205]]

    lsNode.createObject("PointsFromIndices", template="Vec3d", name="posConstraintValues", position="@/hexa/sampler.position", indices=SofaPython.Tools.listToStr(handle_nodes))

    lsNode.createObject("EulerImplicitSolver")
    lsNode.createObject("CGLinearSolver", iterations="25", tolerance="1e-05", threshold="1e-05")

    lsNode.createObject("JacobsonEnergyComputationEngine", template="Vec3d", name="Energy", position="@/hexa/sampler.position")
    lsNode.createObject("JacobsonWeightComputationEngine", template="Vec3d", name="Weights", position="@/hexa/sampler.position", Q="@Energy.Q", posConstraints=SofaPython.Tools.listToStr(handle_nodes), regionConstraints=SofaPython.Tools.listToStr(handle_regions))

    lsNode.createObject("LSJacobsonShapeFunction", name="shapeFunction", W="@Weights.W", position="@/hexa/sampler.position", nbPointParent=len(handle_nodes))

    pointNode = lsNode.createChild("dofPoint")
    pointNode.createObject("PointsFromIndices", name="initPosition", position="@/hexa/sampler.position", indices=SofaPython.Tools.listToStr(handle_nodes))
    pointNode.createObject("MechanicalObject", template="Vec3", name="dof", position="@initPosition.indices_position")

    affineNode = lsNode.createChild("dofAffine")
    affineNode.createObject("MechanicalObject", template="Affine", name="dof", size=len(handle_regions))


    mappedNode = pointNode.createChild("mapped")
    affineNode.addChild(mappedNode)

    mappedNode.createObject("MechanicalObject", template="Vec3", position="@/hexa/sampler.position")
    mappedNode.createObject("LSMultiMapping", name="mapping", input1="@"+pointNode.getPathName(), input2="@"+affineNode.getPathName(), output="@.", mapForces=True, mapConstraints=True, mapMasses=True)

#    visualNode = mappedNode.createChild("visual")
#    visualNode.createObject("VisualModel", name="mesh", position="@/hexa/sampler.position", tetrahedra="@tetra/topology.tetrahedra")
#    visualNode.createObject("IdentityMapping")


