import os.path

import Sofa

import SofaPython.sml
import Anatomy.sml.affineRegistration

usePrefactorizedSystem = True

def createScene(rootNode):
    
    model = SofaPython.sml.Model(SofaPython.Tools.localPath( __file__, "left_leg.xml"))
    scene = Anatomy.sml.affineRegistration.SceneRegistration(rootNode, model, SofaPython.Tools.localPath( __file__, "data_sml_registration"))

    scene.param.showRigid=True
    scene.param.showRigidScale=0.1
    scene.param.showOffset=True
    scene.param.showOffsetScale=0.05
    
    scene.param.GPType = "331"
    
    if usePrefactorizedSystem:
        scene.param.behaviorLaw = 'projective'
    else:
        scene.param.behaviorLaw = 'hooke'

    scene.param.ICPstiffness=1
    scene.param.blendingFactor=0.5
    scene.param.outlierThreshold=0
    scene.param.normalThreshold=0
    
    
    if usePrefactorizedSystem : # remove not prefactorizable links
        for c in scene.model.surfaceLinks.values():
            if "sliding" in c.tags or "collision" in c.tags:
                del scene.model.surfaceLinks[c.id]

    scene.createScene()
    
    rootNode.findData('gravity').value='0 0 0'
    rootNode.findData('dt').value='1'

    rootNode.createObject('BackgroundSetting',color='1 1 1')

    if usePrefactorizedSystem :
        rootNode.createObject('ConstantCompliantImplicitSolver',name='ImplicitSolver')
        rootNode.createObject('LDLTSolver',name='LDLTSolver')
        rootNode.createObject('LDLTResponse',constant='True',name='LDLTResponse')
    else:
        rootNode.createObject('EulerImplicitSolver', name='odesolver')
        rootNode.createObject('CGLinearSolver', name='numsolver', iterations='25', tolerance='1e-14', threshold='1e-14')

    for key,r in scene.registrations.iteritems():
        r['sources'][0].node.createObject('PythonScriptController', filename=__file__, classname='stiffnessController')

    scene.dagValidation() # not mandatory, silent if no pb
    
    return rootNode



#---------------------------------------------------------------------------------------------
class stiffnessController(Sofa.PythonScriptController):
    delta=2
    def createGraph(self,node):
        self.ff=node.getObject('ICP')
        if usePrefactorizedSystem==True :
            self.ImplicitSolver=node.getRoot().getObject('ImplicitSolver')
            self.LDLTResponse=node.getRoot().getObject('LDLTResponse')
        return 0
    
    def onScriptEvent(self, senderNode, eventName, data):
        if eventName=='setStiffness':
            self.setStiffness(int(data))
        if eventName=='setBlendingFactor':
            self.setBlendingFactor(float(data))

    def onKeyPressed(self,k):
    #		print ord(k)
        if ord(k)==45:
            self.setStiffness(self.ff.findData('stiffness').value / self.delta)
        if ord(k)==43:
            self.setStiffness(self.ff.findData('stiffness').value * self.delta)
        if ord(k)==59:
            self.setBlendingFactor(self.ff.findData('blendingFactor').value -0.1)
        if ord(k)==58:
            self.setBlendingFactor(self.ff.findData('blendingFactor').value +0.1)
        return 0
    
    def setStiffness(self, val):
        self.ff.findData('stiffness').value = val
        print 'stiffness = '+str(val)
        self.ff.reinit()
        if usePrefactorizedSystem==True :
            self.ImplicitSolver.reinit()
            self.LDLTResponse.reinit()

    def setBlendingFactor(self, val):
        val = 0 if val < 0 else val
        val = 1 if val > 1 else val
        self.ff.findData('blendingFactor').value = val
        print 'blendingFactor = '+str(val)
        self.ff.reinit()

