import SofaPython.Tools
from SofaPython.Tools import listToStr as concat
from SofaPython.Tools import getObjectPath
import Sofa

class ContactObject:
    """ The object1 and object2 expected by the Contact class must follow this ContactObject model.
    """
    def __init__(self):
        self.node = None
        self.dofs = None
        self.visual = None
        self.model = None
        self.topology = None

class Contact:
    
    def __init__(self, name, object1, object2, compliance, object1Roi=None, object2Roi=None, rigidPosition1=None, rigidPosition2=None, isObject2Polyline=False, restLength=None, N=None, bilateral=True, isCompliance=False, useTangentPlaneProjectionMethod=True, rejectBorder=False, useBV=False, offsetBV=None, parallel=False, forceProjectionToMeshMapping=False):
        """
        object1 and object2 can be instance of Compliant.StructuralAPI.RigidBody, Flexible.API.Deformable, or any other custom object following the ContactObject class.
        :param N: number of contacts, None to get all
        :param rigidPosition2: allows acceleration if object2 is rigid
        """
        self.node = object1.node.createChild(name)

        # get either mechanical or non-mechanical topologies/positions, for either deformable or rigid objects
        pos1=getPosition(object1)
        pos2=getPosition(object2)
        topo2=getOrCreateTopology(object2)

        # find/generate object2 normals
        self.object2Normals = None
        if useTangentPlaneProjectionMethod:
            for obj in object2.node.getObjects():
                if obj.name == 'NormalsFromPoints' or obj.name == 'normalsFromPoints':
                    self.object2Normals = obj
            if self.object2Normals is None:
                if object2.node.isInitialized():
                    self.object2Normals = object2.node.createObject('NormalsFromPoints', name="NormalsFromPoints", warning=False, template="Vec3", position=pos2.getLinkPath()+".position", triangles=topo2.getLinkPath()+".triangles", quads=topo2.getLinkPath()+".quads")
                    self.object2Normals.init()
                else:
                    self.object2Normals = object2.node.createObject('NormalsFromPoints', name="NormalsFromPoints", template="Vec3", position=pos2.getLinkPath()+".position", triangles=topo2.getLinkPath()+".triangles", quads=topo2.getLinkPath()+".quads")

        args={}
        if not rigidPosition1 is None and isMechanical(object1):
            topo1=getTopology(object1)
            args['sourcePosition']=topo1.getLinkPath()+".position"
            args['sourceRigidPosition']=rigidPosition1
        else:
            args['sourcePosition']=pos1.getLinkPath()+".position"

        if not rigidPosition2 is None and isMechanical(object2):
            args['targetPosition']=topo2.getLinkPath()+".position"
            args['targetRigidPosition']=rigidPosition2
        else:
            args['targetPosition']=pos2.getLinkPath()+".position"

        if not object1Roi is None:
            args['sourceROI']=concat(object1Roi)
        if not object2Roi is None:
            args['targetROI']=concat(object2Roi)
        args['useBV']=useBV
        if useBV and not offsetBV is None:
            args['offsetBV']=offsetBV
            if not restLength is None and float(restLength)>float(offsetBV):
                Sofa.msg_error('ContactMapping.API',"restLength>offsetBV -> some contacts can be lost")
        args['parallel']=parallel

        if rejectBorder:
            # find/generate object2 boundary
            self.object2Boundary = None
            for obj in object2.node.getObjects():
                if obj.name == 'MeshBoundaryROI':
                    self.object2Boundary = obj
            if self.object2Boundary is None:
                if not object2Roi is None:
                    self.object2Boundary = object2.node.createObject('MeshBoundaryROI', name="MeshBoundaryROI", inputROI=concat(object2Roi), triangles=topo2.getLinkPath()+".triangles", quads=topo2.getLinkPath()+".quads")
                else:
                    self.object2Boundary = object2.node.createObject('MeshBoundaryROI', name="MeshBoundaryROI", triangles=topo2.getLinkPath()+".triangles", quads=topo2.getLinkPath()+".quads")
            if rejectBorder and useTangentPlaneProjectionMethod:
                args['rejectROI']=self.object2Boundary.getLinkPath()+".indices"

        self.closestPoints = self.node.createObject('ClosestPointEngine', name="ClosestPointEngine", **args)

        argsMapping={'indexPairs':"@ClosestPointEngine.indexPairs", 'output':"@.", 'closests':"@projectionToMeshEngine.output"}
        args={'template':"Vec3", 'indexPairs':"@ClosestPointEngine.indexPairs",'useRestLength':True}

        if not restLength is None:
            args['restLength']=restLength

        isMultiMapping = None

        if isMechanical(object1) and isMechanical(object2):
            argsMapping['input']= object1.node.getLinkPath()+"/. "+object2.node.getLinkPath()+"/."
            args['sourcePosition'] = object1.dofs.getLinkPath()+".position"
            args['targetPosition'] = object2.dofs.getLinkPath()+".position"
            if useTangentPlaneProjectionMethod:
                args['normals2']= self.object2Normals.getLinkPath()+".normals"
            else:
                if isObject2Polyline:
                    args['edges2']= topo2.getLinkPath()+".edges"
                else:
                    args['triangles2']= topo2.getLinkPath()+".triangles"
                    args['quads2']= topo2.getLinkPath()+".quads"

            isMultiMapping = True

        elif isMechanical(object1) and not isMechanical(object2):
            argsMapping['input']= object1.node.getLinkPath()+"/."
            args['sourcePosition'] = object1.dofs.getLinkPath()+".position"
            args['targetPosition']= pos2.getLinkPath()+".position"
            if useTangentPlaneProjectionMethod:
                args['normal']= self.object2Normals.getLinkPath()+".normals"
            else:
                if isObject2Polyline:
                    args['edges2']= topo2.getLinkPath()+".edges"
                else:
                    args['triangles']= topo2.getLinkPath()+".triangles"
                    args['quads']= topo2.getLinkPath()+".quads"

            isMultiMapping = False

        elif not isMechanical(object1) and isMechanical(object2):
            argsMapping['input']= object2.node.getLinkPath()+"/."
            args['sourcePosition'] = object2.dofs.getLinkPath()+".position"
            args['targetPosition']= pos1.getLinkPath()+".position"

            self.object1Normals = None
            if useTangentPlaneProjectionMethod:
                # find/generate object1 normals
                for obj in object1.node.getObjects():
                    if obj.name == 'NormalsFromPoints':
                        self.object1Normals = obj
                if self.object1Normals is None:
                    topo1=getOrCreateTopology(object1)
                    if object1.node.isInitialized():
                        self.object1Normals = object1.node.createObject('NormalsFromPoints', name="NormalsFromPoints", warning=False, template="Vec3", position=pos1.getLinkPath()+".position", triangles=topo1.getLinkPath()+".triangles", quads=topo1.getLinkPath()+".quads")
                        self.object1Normals.init()
                    else:
                        self.object1Normals = object1.node.createObject('NormalsFromPoints', name="NormalsFromPoints", template="Vec3", position=pos1.getLinkPath()+".position", triangles=topo1.getLinkPath()+".triangles", quads=topo1.getLinkPath()+".quads")
                args['normal']= self.object1Normals.getLinkPath()+".normals"
            else:
                topo1=getTopology(object1)
                args['triangles']= topo1.getLinkPath()+".triangles"
                args['quads']= topo1.getLinkPath()+".quads"

            self.closestPoints.revertPairs = True

            isMultiMapping = False
        else :
            return

        if useTangentPlaneProjectionMethod:
            self.projectionToMeshEngine = self.node.createObject('ProjectionToVertexPlaneEngine', name="projectionToMeshEngine", **args)
        else:
            self.projectionToMeshEngine = self.node.createObject('ProjectionToMeshEngine', name="projectionToMeshEngine", **args)

        validPairs = None

        if rejectBorder and not useTangentPlaneProjectionMethod:
            self.closestRejectROIEngine = self.node.createObject('ClosestRejectROIEngine', name="closestRejectROIEngine", closests="@projectionToMeshEngine.output", rejectROI=self.object2Boundary.getLinkPath()+".indices"  )
            validPairs = self.closestRejectROIEngine.getLinkPath()+".output"

        self.closestKeepNSmallestEngine = None
        if not N is None:
            self.closestKeepNSmallestEngine = self.node.createObject('ClosestKeepNSmallestEngine', name="closestKeepNSmallestEngine", template="Vec3",
                                                                     closests = "@projectionToMeshEngine.output",
                                                                     indexPairs = "@ClosestPointEngine.indexPairs",
                                                                     keepNSmallest = N,
                                                                     bilateral = bilateral)
            if validPairs is not None:
                self.closestKeepNSmallestEngine.validPairs = validPairs
            validPairs = self.closestKeepNSmallestEngine.getLinkPath()+".output"

        if validPairs is not None:
            argsMapping['validPairs']=validPairs

        if (isObject2Polyline and not restLength is None and 0==restLength) or forceProjectionToMeshMapping :
            Sofa.msg_info("Using ProjectionToMesh for "+name)
            MappingType = "ProjectionToMesh" + ("Multi" if isMultiMapping else "") + "Mapping"
            self.node.createObject('MechanicalObject', template="Vec3", name="dofs")
            self.mapping = self.node.createObject(MappingType, template="Vec3,Vec3", name="mapping", **argsMapping)
            nodeDifference = self.node.createChild("difference")
            self.dofs = nodeDifference.createObject('MechanicalObject', template="Vec3", name="dofs")
            nodeDifference.createObject("DifferenceMultiMapping", template="Vec3,Vec3", name="mapping",
            input=argsMapping['input'].split(' ')[0]+" @../", output="@./", unidirectional=1)
            self.compliance = nodeDifference.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=isCompliance, resizable=True)
        else:
            Sofa.msg_info("Using DistanceToMesh for "+name)
            if isObject2Polyline and not restLength is None and 0==restLength:
                Sofa.msg_warning("Using DistanceToMesh for polyline with restLength=0")
            self.dofs = self.node.createObject('MechanicalObject', template="Vec1", name="dofs")
            MappingType = "DistanceToMesh" + ("Multi" if isMultiMapping else "") + "Mapping"
            self.mapping = self.node.createObject(MappingType, template="Vec3d,Vec1d", name="mapping", **argsMapping)
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=isCompliance, resizable=True)
            if bilateral is False:
                self.constraintvalue = self.node.createObject('UnilateralConstraint')
                self.compliance.isCompliance=True
                self.compliance.resizable=True
        if rejectBorder:
            self.compliance.resizable=True
        # self.node.createObject('Stabilization') # you do not want to add Stabilization if compliance > 0, this constraint type is handled automatically by the solver depending on the compliance value

        object2.node.addChild(self.node)

    def ignoreInitialPenetratedVertex(self):
        self.node.createObject('PythonScriptController', name="ignoreInitialPenetratedVertex", filename=SofaPython.Tools.localPath(__file__, "scriptController.py"), classname="IgnorePenetratedVertex")




def getVisual(object):
    if hasattr(object, 'visual') and not object.visual is None:
        return object.visual
    elif hasattr(object, 'model') and not object.model is None:
        return object.model
    else:
        Sofa.msg_error('ContactMapping.API',"can't find object VisualModel")
        return None


def getOrCreateTopology(object):
    if hasattr(object, 'topology') and not object.topology is None:
        return object.topology
    # NO! never returns a VisualModel as a Topology
    # visual model face indices can be larger than the position vector
    # due to several superimposed points for several normals ot texcoords
    # elif hasattr(object, 'visual') and not object.visual is None:
    #     return object.visual
    # elif hasattr(object, 'model') and not object.model is None:
    #     return object.model
    else:

        visual = getVisual(object)
        if visual is not None:
            return object.node.createObject('MeshTopology', filename=visual.getLinkPath()+".filename")

        Sofa.msg_error('ContactMapping.API',"can't find or create object topology")
        return None

def getTopology(object):
    if hasattr(object, 'topology') and not object.topology is None:
        return object.topology
    # NO! never returns a VisualModel as a Topology
    # visual model face indices can be larger than the position vector
    # due to several superimposed points for several normals ot texcoords
    # elif hasattr(object, 'visual') and not object.visual is None:
    #     return object.visual
    # elif hasattr(object, 'model') and not object.model is None:
    #     return object.model
    else:
        Sofa.msg_error('ContactMapping.API',"can't find object topology")
        return None


def getPosition(object):
    if hasattr(object, 'dofs') and not object.dofs is None:
        return object.dofs
    elif hasattr(object, 'visual') and not object.visual is None:
        return object.visual
    elif hasattr(object, 'model') and not object.model is None:
        return object.model
    else:
        Sofa.msg_error('ContactMapping.API',"can't find object position")
        return None

def isMechanical(object):
    if hasattr(object, 'dofs') and not object.dofs is None:
        return True
    else :
        return False
