import sys
import os.path

import Sofa
import SofaPython.Tools
import SofaPython.units
import SofaPython.sml
import Compliant.sml
import ContactMapping.StructuralAPI
import ContactMapping.sml

def createScene(rootNode):
    rootNode.createObject('RequiredPlugin', name="Compliant")
    rootNode.createObject('RequiredPlugin', name="ContactMapping")

    rootNode.createObject('VisualStyle', displayFlags='showMechanicalMappings showBehaviorModels')

    rootNode.dt=0.003
    rootNode.gravity=[0,0,0]

    rootNode.createObject('CompliantAttachButtonSetting', compliance=0.001)
    
    rootNode.createObject('CompliantImplicitSolver', name="odesolver", stabilization=True)
    
    rootNode.createObject('LDLTSolver', name="numsolver")
    
    model = SofaPython.sml.Model(os.path.join(os.path.dirname(__file__),"data/knee.sml"))
    
    scene = Compliant.sml.SceneArticulatedRigid(rootNode, model)
    scene.param.showRigid=True
    scene.param.showRigidScale=0.02
    scene.createScene()
    
    scene.rigids["femur"].node.createObject('FixedConstraint')
    
    scene.param.contactCompliance = 1e-2

    contact_01 = ContactMapping.sml.insertContact(model.surfaceLinks["tibia_femur_01"], scene, compliance=scene.param.contactCompliance, rejectBorder=True, useTangentPlaneProjectionMethod=True, restLength = SofaPython.units.length_from_SI(0.005), N=10)
    contact_01.closestPoints.printLog=True
    contact_01.projectionToMeshEngine.printLog=True
    contact_01.mapping.printLog=True
    scene.rigids["tibia_01"].collision.visual.model.setColor(0,0,1,1)

    contact_02 = ContactMapping.sml.insertContact(model.surfaceLinks["tibia_femur_02"], scene, compliance=scene.param.contactCompliance, rejectBorder=True, useTangentPlaneProjectionMethod=False, restLength = SofaPython.units.length_from_SI(0.005), N=10)
    contact_02.closestPoints.printLog=True
    contact_02.projectionToMeshEngine.printLog=True
    contact_02.mapping.printLog=True
    scene.rigids["tibia_02"].collision.visual.model.setColor(0,1,0,1)
    


    return rootNode
