import Sofa
# import ContactMapping
import sys

def createSceneAndController(rootNode):
    rootNode.createObject('RequiredPlugin', pluginName="ContactMapping")

    rootNode.createObject('MeshObjLoader', name='sphere', filename='mesh/sphere.obj', triangulate='1', translation='0 0 -3', scale3d='2 2.5 3')
    rootNode.createObject('VisualModel', src="@sphere")
    rootNode.createObject('MeshObjLoader', name='cube', filename='mesh/cube.obj', translation="0 0 1")
    rootNode.createObject('VisualModel', src="@cube")


    rootNode.createObject('ClosestPointEngine', name="cp", template="Vec3", sourcePosition="@cube.position", targetPosition="@sphere.position", printLog=False,drawMode="1")

    global engine
    engine = rootNode.createObject('ProjectionToMeshEngine', name="pm", template="Vec3", printLog=True,
                        sourcePosition="@cp.sourcePosition", targetPosition="@cp.targetPosition",
                        indexPairs="@cp.indexPairs" ,
                        triangles="@sphere.triangles",
                        useRestLength=False )

    #
    # engine = rootNode.createObject('ClosestKeepNSmallestEngine', name="kn", template="Vec3", printLog=True,
    #                 closests="@pm.output",
    #                 indexPairs="@cp.indexPairs",
    #                 keepNSmallest=5,
    #                 bilateral=False )

    return rootNode


def bwdInitGraph(node):
    print type(engine.findData('output'))
    print type(engine.findData('output').value)
    print type(engine.output)

    # get
    p = engine.output.indexWeights
    print p
    # set
    p[-1]=[[0,1]]
    engine.output.indexWeights =p
    print engine.output.indexWeights

    # get
    p = engine.output.dist
    print p
    # set
    p[-1]=0
    engine.output.dist =p
    print engine.output.dist

    # get
    p = engine.output.isPlane
    print p
    # set
    p[-1]=0
    engine.output.isPlane =p
    print engine.output.isPlane

    # get
    p = engine.output.u
    print p
    # set
    p[-1]=[1,2,3]
    engine.output.u =p
    print engine.output.u

    # get
    p = engine.output.n
    print p
    # set
    p[-1]=[1,2,3]
    engine.output.n =p
    print engine.output.n

    sys.stdout.flush()
