import StructuralAPI
import SofaPython.Tools
import Sofa

printLog = True

def _insertContact(contactModel,scene,doInvertSurfaces=False,**kwargs):
    if not doInvertSurfaces:
        surface1 = contactModel.surfaces[0]
        surface2 = contactModel.surfaces[1]
    else:
        surface1 = contactModel.surfaces[1]
        surface2 = contactModel.surfaces[0]

    mesh1=scene.getCollision(surface1.solid.id,surface1.mesh.id)
    mesh2=scene.getCollision(surface2.solid.id,surface2.mesh.id)

    if not mesh1:
        mesh1=scene.getVisual(surface1.solid.id,surface1.mesh.id)
        if not mesh1:
            Sofa.msg_error('ContactMapping.sml',surface1.mesh.id+" should be a collision or visual model")
    if not mesh2:
        mesh2=scene.getVisual(surface2.solid.id,surface2.mesh.id)
        if not mesh2:
            Sofa.msg_error('ContactMapping.sml',surface2.mesh.id+" should be a collision or visual  model")
    if mesh1 and mesh2:
        object1Roi = None if surface1.group is None else surface1.mesh.group[surface1.group].index
        object2Roi = None if surface2.group is None else surface2.mesh.group[surface2.group].index

        rigidPosition1=None if not surface1.solid.id in scene.rigids else '@'+SofaPython.Tools.getObjectPath(scene.rigids[surface1.solid.id].dofs)+'.position'
        rigidPosition2=None if not surface2.solid.id in scene.rigids else '@'+SofaPython.Tools.getObjectPath(scene.rigids[surface2.solid.id].dofs)+'.position'

        contact = StructuralAPI.Contact( name= contactModel.name+"__inverted" if doInvertSurfaces else contactModel.name, object1 = mesh1, object2 = mesh2,
                                         object1Roi=object1Roi, object2Roi=object2Roi,
                                         rigidPosition1=rigidPosition1, rigidPosition2=rigidPosition2, **kwargs)
        return contact

def insertContact(contactModel,scene,symmetricContact=False,**kwargs):
    #  @todo: convert units? compliances on distances are in s^2/kg

    if printLog:
        Sofa.msg_info('ContactMapping.sml', 'insertContact : '+ contactModel.name)

    if symmetricContact :
        return (_insertContact(contactModel, scene, **kwargs), _insertContact(contactModel, scene, doInvertSurfaces=True, **kwargs))
    else:
        return _insertContact(contactModel, scene, **kwargs)


