import os.path

import Sofa

import SofaPython.sml
import Anatomy.sml.rigidFem
import Compliant.sml
import SofaPython.Tools

def createSceneAndController(rootNode):
    
    global scene
    scene=create(rootNode,'sceneCoarse',0.02,4)

    rootNode.findData('gravity').value='10 0 0'
    rootNode.findData('dt').value='0.1'
    rootNode.createObject('BackgroundSetting',color='1 1 1')
    
    global odesolver
    # odesolver = rootNode.createObject('CompliantImplicitSolver', name='odesolver',stabilization=0)
    odesolver = rootNode.createObject('CompliantPseudoStaticSolver', name='odesolver', iterations="1")
    rootNode.createObject('SequentialSolver', name="numsolver", iterations =250, precision=1e-14)
    # rootNode.createObject('LDLTSolver', name='numsolver')
    # rootNode.createObject('CgSolver', name='numsolver', iterations='30', precision='1e-10' )

    # scene.dagValidation() # not mandatory, silent if no pb
    # Sofa.forceInitNodeCreatedInPython()

    print "Type Ctrl+ to augment resolution"
    return rootNode


def onKeyPressed(k):
    print 'onKeyPressed '+k
    sys.stdout.flush()

    global scene
    if k=='+':
        if scene.param.coarseningFactor>1:
            rootNode = scene.node.getParents()[0].getParents()[0]
            newScene=create(rootNode,'scene_'+str(scene.param.coarseningFactor-1),scene.param.voxelSize,scene.param.coarseningFactor-1)
            Anatomy.sml.rigidFem.mapFineScene(rootNode,scene,newScene)
            scene = newScene
            print scene.param.coarseningFactor

    # if k=='-':
    #     rootNode = scene.node.getParents()[0].getParents()[0]
    #     global odesolver
    #     rootNode.removeObject(odesolver)
    #     # odesolver = rootNode.createObject('CompliantImplicitSolver', name='odesolver',stabilization=0)
    #     odesolver = rootNode.createObject('CompliantPseudoStaticSolver', name='odesolver', iterations="1")
    #     odesolver.init()

    return 0


def create(rootNode, name, voxelSize, coarseningFactor):
    model = SofaPython.sml.Model(SofaPython.Tools.localPath( __file__, "left_leg.xml"))
    node = rootNode.createChild(name)
    scene = Anatomy.sml.rigidFem.Scene(node, model)
    scene.param.showRigid=True
    scene.param.showRigidScale=0.1
    # scene.param.showGPScale=0.001
    scene.param.voxelSize=voxelSize
    scene.param.coarseningFactor=coarseningFactor
    scene.param.showHexa =True
    scene.param.behaviorLaw = 'hooke'
    scene.param.gridType='branching'
    scene.param.collisionNbClosestVertex = 10
    scene.createScene()
    scene.rigids["l_femur"].node.createObject('FixedConstraint')
    return scene