import os.path

import Sofa

import SofaPython.sml
import Anatomy.sml.rigidAffine
import Anatomy.Tools
import ContactMapping.StructuralAPI

def createSceneAndController(node):
    
    model = SofaPython.sml.Model(SofaPython.Tools.localPath( __file__, "spine.xml"))
    
    global scene
    scene = Anatomy.sml.rigidAffine.SceneSimu(node, model, SofaPython.Tools.localPath( __file__, "data_sml"))

    scene.param.showRigid=False
    scene.param.showRigidScale=0.05
    scene.param.showOffset=False
    scene.param.showOffsetScale=0.02
    scene.param.showAffine=False
    
    scene.param.GPType = "332"

    scene.createScene()
    node.createObject("OrderIndependentTransparencyManager")
    node.createObject("RequiredPlugin", name="ContactMapping")
    node.createObject("VisualStyle", displayFlags="showVisualModels hideCollisionModels showBehaviorModels")
    node.createObject('CompliantAttachButtonSetting' )
    node.createObject('CompliantPseudoStaticSolver', name='odesolver', iterations=1, neglecting_compliance_forces_in_geometric_stiffness=False)
    node.createObject('SequentialSolver', name='numsolver', iterations=250, precision=1e-14, iterateOnBilaterals=True)

    node.findData('gravity').value='0 0 0'

    node.createObject('BackgroundSetting',color='1 1 1')
    
    #scene.rigids["V5"].setFixed()
    
    spine = ContactMapping.StructuralAPI.ContactObject()
    
    spine.node = node.createChild("allVertebraCenters")
    vertebraCenters=dict()
    allInput=""
    allIndexPairs=""
    i=0
    # map a point at the origin of each rigid
    for id,rigid in scene.rigids.iteritems():
        vertebraCenters[id] = rigid.addAbsoluteMappedPoint("center", position=rigid.framecom.translation)
        vertebraCenters[id].node.addChild(spine.node)
        allInput += '@'+vertebraCenters[id].node.getPathName()+" "
        allIndexPairs += str(i) + " 0 "
        i+=1
    # gather these points in a unique dof
    spine.dofs = spine.node.createObject("MechanicalObject", template="Vec3")
    spine.dofs.showObject = True
    spine.dofs.showObjectScale = 0.01
    spine.dofs.drawMode = 1
    spine.node.createObject('SubsetMultiMapping', template = "Vec3,Vec3", name="mapping", input = allInput , output = '@./', indexPairs = allIndexPairs, applyRestPosition=True )
    
    # load a "postural" spline
    spline = ContactMapping.StructuralAPI.ContactObject()
    spline.node = node.createChild("spline")
    spline.node.createObject("MeshObjLoader", name="loader", filename="spline.obj")
    spline.dofs = spline.node.createObject("MechanicalObject", src="@loader")
    spline.topology = spline.node.createObject("MeshTopology", name="topology", src="@loader", drawEdges=True)
    spline.node.createObject("FixedConstraint", fixAll=True)
    
    # create the constraint between the vertebra centers and the spline
    spinePostureContact = ContactMapping.StructuralAPI.Contact("spine_posture", spine, spline, compliance=1e-4, restLength=0, useTangentPlaneProjectionMethod=False, rejectBorder=False, isObject2Polyline=True)
    spinePostureContact.closestPoints.drawMode=1

    return node
