import sys
import math
import Sofa
import Compliant.Frame
import Compliant.StructuralAPI
import Anatomy.Constraint
import SofaPython.Quaternion

def setShowDofs(dofs, scale=0.5, mode=0):
    dofs.showObject = True
    dofs.showObjectScale = scale
    dofs.drawMode = mode

def createScene(rootNode):
    global constraint
    constraint=dict()
    
    rootNode.createObject('RequiredPlugin', pluginName="Compliant")
    rootNode.createObject('RequiredPlugin', pluginName="Anatomy")
    
    rootNode.createObject('PythonScriptController', name="script", filename=__file__, classname="PythonScript")
    
    rootNode.createObject('CompliantImplicitSolver', name='odesolver', stabilization="true")
    
    rootNode.createObject('SequentialSolver', name='numsolver', iterations=250, precision=1e-14, iterateOnBilaterals=True)
    #rootNode.createObject('LDLTSolver')
    rootNode.createObject('LDLTResponse', name='response')

    rootNode.gravity=[0,0,0]
    
    rigidRigidTransformationNode = rootNode.createChild("RigidRigidTransformation")
    
    nodeWorld = rigidRigidTransformationNode.createChild("World")
    W = Compliant.Frame.Frame([0,0,0,0,0,0,1])
    W.dofs = W.insert(nodeWorld, name="dofs_fixed")
    nodeWorld.createObject('RigidMass', name = 'mass', mass = 1, inertia="1 1 1")
    setShowDofs(W.dofs,.5,0)
    nodeWorld.createObject('FixedConstraint')
    
    # W to A : full control
    nodeA = rigidRigidTransformationNode.createChild("A")
    A = Compliant.Frame.Frame([1,0.5,0]+SofaPython.Quaternion.rotVecToQuat([math.radians(30), math.radians(60), 0]))
    A.dofs = A.insert(nodeA, name="dofs_controlled")
    nodeA.createObject('RigidMass', name = 'mass', mass = 1, inertia="1 1 1")
    setShowDofs(A.dofs,.5,0)
    
    constraint["W_to_A"] = Anatomy.Constraint.RigidRigidTransformation("W_to_A", W, A, [1,1,1,1,1,1])
    
    # W to B : one translation
    nodeB = rigidRigidTransformationNode.createChild("J")
    B = Compliant.Frame.Frame([2,0.5,0]+SofaPython.Quaternion.rotVecToQuat([math.radians(45), math.radians(70), 15]))
    B.dofs = B.insert(nodeB, name="dofs_controlled")
    nodeB.createObject('RigidMass', name = 'mass', mass = 1, inertia="1 1 1")
    setShowDofs(B.dofs,.5,0)
    
    constraint["W_to_B"] = Anatomy.Constraint.RigidRigidTransformation("W_to_B", W, B, [1,0,0,0,0,0])

class PythonScript(Sofa.PythonScriptController) :
    def setTarget(self, name, target) :
        global constraint
        print "DEBUG", "setTarget", name, target
        sys.stdout.flush()
        constraint[name].setTarget(target)
    def getTarget(self, name) :
        global constraint
        print "DEBUG", "getTarget", name, constraint[name].getTarget()
        sys.stdout.flush()
        return constraint[name].getTarget()