import Sofa
from Compliant import StructuralAPI, Tools
import SofaPython.Quaternion as quat

def createScene(rootNode):
    
    rootNode.gravity=[0,0,0]
    rootNode.createObject('RequiredPlugin', pluginName="Compliant")
    rootNode.createObject('RequiredPlugin', pluginName="ContactMapping")
    
    rootNode.createObject('BackgroundSetting', color='1 1 1')
#    rootNode.createObject('VisualStyle', displayFlags='showMechanicalMappings')

    rootNode.dt=0.1
    rootNode.gravity=[0,0,0]
    
    rootNode.createObject('CompliantAttachButtonSetting', compliance=1E-10)
    
    rootNode.createObject('CompliantImplicitSolver', name="odesolver", stabilization=True)
#    rootNode.createObject('SequentialSolver', name="numsolver", iterations =10, precision=1e-14)
    rootNode.createObject('LDLTSolver', name="numsolver")

    t = StructuralAPI.RigidBody( rootNode, "target" )
    t.setManually(offset = [0,0,0,0,0,0,1] )
    # t.addCollisionMesh( 'mesh/raptor_35kp.obj', scale3d=[.2,.15,0.08] ) # stressing the registration a bit with different models
    t.addCollisionMesh( 'mesh/raptor_35kp.obj', scale3d=[.1,.1,.1] ) # similar models
    t.collision.addNormals()
    vt=t.collision.addVisualModel()
    vt.model.setColor(1,0,0,1)
#    t.setFixed()

    s = StructuralAPI.RigidBody( rootNode, "source" )
    s.setManually(offset = [1,0,0]+quat.from_euler([1,1,1]).tolist() )
    s.addCollisionMesh( 'mesh/raptor_8kp.obj', scale3d=[.1,.1,.1] )
    s.collision.addNormals()
    vs=s.collision.addVisualModel()
    vs.model.setColor(0,1,0,1)

    addConstraint(s,t)
    addConstraint(t,s)

    return rootNode


def addConstraint(r1,r2):

    distance = r1.collision.node.createChild("distance")
    distance.createObject('MechanicalObject', template="Vec1d", name="dof")
    
    distance.createObject('ClosestPointEngine', name="cp", parallel=True, sourcePosition="@"+r1.collision.node.getPathName()+"/topology.position", sourceRigidPosition="@"+r1.node.getPathName()+"/dofs.position", targetPosition="@"+r2.collision.node.getPathName()+"/topology.position", targetRigidPosition="@"+r2.node.getPathName()+"/dofs.position", printLog=True,drawMode="0",method="1",useBV=False, cacheSize=5, targetROISize=5000, sourceROISize=5000)
    # distance.createObject('ClosestPointEngine', name="cp", parallel=True, sourcePosition="@"+r1.collision.node.getPathName()+"/dofs.position", targetPosition="@"+r2.collision.node.getPathName()+"/dofs.position",  printLog=True,drawMode="0",method="1",useBV=False, cacheSize=100, targetROISize=0, sourceROISize=0)


    # projection to mesh -> preserves momentum
    distance.createObject('ProjectionToMeshEngine', name="pm", printLog=True, template="Vec3d",
                    sourcePosition="@"+r1.collision.dofs.getPathName()+".position",
                    targetPosition="@"+r2.collision.dofs.getPathName()+".position",
                    indexPairs="@cp.indexPairs" ,
                    triangles2="@"+r2.collision.node.getPathName()+"/topology.triangles", quads2="@"+r2.collision.node.getPathName()+"/topology.quads",
                    useRestLength=True, restLength='0' )

    # projection to tangent planes -> ghost momentum
    # distance.createObject('ProjectionToVertexPlaneEngine', name="pm",
    #                 sourcePosition="@"+r1.collision.dofs.getPathName()+".position",
    #                 targetPosition="@"+r2.collision.dofs.getPathName()+".position",
    #                 indexPairs="@cp.indexPairs" ,
    #                 normals2="@"+r2.collision.node.getPathName()+"/normalsFromPoints.normals",
    #                 useRestLength=True, restLength='0' )

    # two ways
    distance.createObject('DistanceToMeshMultiMapping', template="Vec3d,Vec1d", name="mapping", input="@"+r1.collision.node.getPathName()+" @"+r2.collision.node.getPathName(), output="@.", indexPairs="@cp.indexPairs", closests="@pm.output" , printLog=True)

    # one way using projection to mesh
    # distance.createObject('DistanceToMeshMapping', template="Vec3d,Vec1d", name="mapping", input="@"+r1.collision.node.getPathName(), output="@.", indexPairs="@cp.indexPairs", position="@"+r2.collision.node.getPathName()+"/topology.position", closests="@pm.output", printLog=True)

    distance.createObject('UniformCompliance', compliance=1E-2,rayleighStiffness=0.1,isCompliance='0',resizable=True)
    r2.collision.node.addChild(distance)



