## @package StructuralAPI
# An alternative high(mid?)-level python API to describe a Compliant scene.
#
# With this API, the SOFA structure is not hidden and must be known by the user.
# But the obscure python-based structures (like Frame) are hidden.
#
# An important advantage is that a pointer is accessible for any created node and component
# (simplifying component customization, manual sub-scene creation, etc.)
#
# Note that both python APIs and manual creation can be used together.
#
# see Compliant/examples/StructuralAPI.py for a basic example.

import Frame
import Tools
from Tools import cat as concat
import numpy
import Sofa
from SofaPython import Quaternion
import SofaPython.Tools
import SofaPython.mass
import math
import sys


from SofaPython import SofaNumpy as sofa_numpy
from SofaPython.SofaNumpy import numpify as numpy_data
from types import Rigid3

import numpy as np

# to specify the floating point encoding:
# "d" to force double
# "f" to force float
# "" to let the template aliases mechanism chose according sofa compilation option (USE_FLOAT and USE_DOUBLE)
template_suffix=""

# global variable to give a different name to each visual model
idxVisualModel = 0

# to use geometric_stiffness of rigid mappings
# @warning WIP, the API will change
geometric_stiffness = 0

def scaleOffset(scale, offset):
    """ scale the translation part of an offset
    """
    return (scale*numpy.asarray(offset[0:3])).tolist() + numpy.asarray(offset[3:]).tolist()



def numpy_property(attr_name, attr_data, index = 0):
    '''easily bind data properties as attributes'''

    def getter(self):
        obj = getattr(self, attr_name)
        return numpy_data(obj, attr_data)[index]

    def setter(self, value):
        obj = getattr(self, attr_name)
        numpy_data(obj, attr_data)[index] = value
        
    return property(getter, setter)



class DOFs(object):
    '''state vector view for a mechanical object'''
    
    __slots__ = ('dofs', )
    
    def __init__(self, dofs):
        self.dofs = dofs
        
    @property
    def position(self):
        return numpy_data(self.dofs, 'position')
    
    @position.setter
    def position(self, value):
        self.position[:] = value


    @property
    def velocity(self):
        return numpy_data(self.dofs, 'velocity')
    
    @velocity.setter
    def velocity(self, value):
        self.velocity[:] = value


    @property
    def force(self):
        return numpy_data(self.dofs, 'force')
    
    @force.setter
    def force(self, value):
        self.force[:] = value


    @property
    def external_force(self):
        return numpy_data(self.dofs, 'externalForce')
    
    @external_force.setter
    def external_force(self, value):
        self.external_force[:] = value

        
    @property
    def template(self):
        return self.dofs.getTemplateName()


    @property
    def show(self):
        return self.dofs.showObject

    @show.setter
    def show(self, value):
        self.dofs.showObject = value


    @property
    def context(self):
        return self.dofs.getContext()


def single_dof(cls):
    '''decorator for single dof views'''

    class Result(cls):
        __slots__ = ()

        @cls.position.getter
        def position(self):
            return super(Result, self).position[0]


        @cls.velocity.getter
        def velocity(self):
            return super(Result, self).velocity[0]        


        @cls.force.getter
        def force(self):
            return super(Result, self).force[0]        


        @cls.external_force.getter
        def external_force(self):
            return super(Result, self).external_force[0]            
        
    return Result


@single_dof
class DOF(DOFs):
    '''single dof view'''
    pass



def dofs_type(coord_type, deriv_type = None):
    '''gives properly typed access to decorated class'''
    
    deriv_type = deriv_type or coord_type.Deriv
    
    def decorator(cls):

        # TODO maybe just add these to the class instead
        class Result(cls):
            __slots__ = ()

            @cls.position.getter
            def position(self):
                return super(Result, self).position.view(coord_type)


            @cls.velocity.getter
            def velocity(self):
                return super(Result, self).velocity.view(deriv_type)  


            @cls.force.getter
            def force(self):
                return super(Result, self).force.view(deriv_type)       


            @cls.external_force.getter
            def external_force(self):
                return super(Result, self).external_force.view(deriv_type)
        
        return Result

    return decorator





@dofs_type(Rigid3)
@single_dof
class MappedRigid(DOFs):
    '''rigid dof mapped from another rigid'''
    
    __slots__ = 'mapping', 'parent'
    
    def __init__(self, dofs, mapping):
        DOFs.__init__(self, dofs)
        self.mapping = mapping
        assert mapping.getClassName() == 'AssembledRigidRigidMapping'

    @property
    def offset(self):
        import ctypes
        ptr, shape, typename = self.mapping.findData('source').getValueVoidPtr()
        array = (ctypes.c_double * 7).from_address(ptr)

        return np.ctypeslib.as_array( array ).view(Rigid3)

    @offset.setter
    def offset(self, value):
        self.offset[:] = value
        
    @property
    def parent_context(self):
        return self.context.getParents()[0]
    

@dofs_type(Rigid3)
@single_dof
class RigidDOF(DOFs):
    '''rigid single dof view'''
    
    def map_rigid(self, name):
        node = self.context.createChild(name)

        dofs = node.createObject('MechanicalObject', template = 'Rigid3', size = 1)
        mapping = node.createObject('AssembledRigidRigidMapping', template = 'Rigid3,Rigid3',
                                    input = self.dofs.getLinkPath(),
                                    output = dofs.getLinkPath(),
                                    source = '0 0 0 0 0 0 0 1')
        
        return MappedRigid(dofs, mapping)

    # TODO map_points
    

    
class RigidBody(RigidDOF):
    '''a rigid body with single dof'''

    @property
    def total_mass(self):
        return self.mass.mass[0]

    @total_mass.setter
    def total_mass(self, value):
        self.mass.mass = value


    @property
    def inertia(self):
        return self.mass.inertia[0]

    @inertia.setter
    def inertia(self, value):
        self.mass.inertia = value

    @property
    def inertia_forces(self):
        return self.mass.inertia.inertia_forces

    @inertia_forces.setter
    def inertia_forces(self, value):
        self.mass.inertia_forces = value


    

    # TODO provide accessors to the above for user frame (offseted by
    # com/inertia/offset)
    

    def __init__(self, node, name):
        self.node = node.createChild( name )  # node
        self.collision = None # the added collision mesh if any
        self.visual = None # the added visual model if any
        self.dofs = None   # dofs
        self.mass = None   # mass
        self.frame = Frame.Frame()
        self.framecom = Frame.Frame()
        self.fixedConstraint = None # to fix the RigidBody

    def setFromMesh(self, filepath, density = 1000, offset = [0,0,0,0,0,0,1], scale3d=[1,1,1], inertia_forces = 0 ):
        ## create the rigid body from a mesh (inertia and com are automatically computed)
        massInfo = SofaPython.mass.RigidMassInfo()
        massInfo.setFromMesh(filepath, density, scale3d)
        self.setFromRigidInfo(massInfo, offset, inertia_forces)

    def setFromRigidInfo(self, info, offset = [0,0,0,0,0,0,1], inertia_forces = 0) :
        self.framecom = Frame.Frame()
        self.framecom.rotation = info.inertia_rotation
        self.framecom.translation = info.com

        self.frame = Frame.Frame(offset) * self.framecom

        self.dofs = self.frame.insert( self.node, name = 'dofs', template="Rigid3"+template_suffix )
        self.mass = self.node.createObject('RigidMass',
                                name = 'mass',
                                mass = info.mass,
                                inertia = concat(info.diagonal_inertia),
                                inertia_forces = inertia_forces )

    def setManually(self, offset = [0,0,0,0,0,0,1], mass = 1, inertia = [1,1,1], inertia_forces = 0 ):
        ## create the rigid body by manually giving its inertia
        self.frame = Frame.Frame( offset )
        self.dofs = self.frame.insert( self.node, name='dofs', template="Rigid3"+template_suffix )
        self.mass = self.node.createObject('RigidMass',
                                name = 'mass',
                                mass = mass,
                                inertia = concat(inertia),
                                inertia_forces = inertia_forces )

    def addCollisionMesh(self, filepath, scale3d=[1,1,1], offset=[0,0,0,0,0,0,1], name_suffix=''):
        ## adding a collision mesh to the rigid body with a relative offset
        # (only a Triangle collision model is created, more models can be added manually)
        # @warning the translation due to the center of mass offset is
        # automatically removed. If necessary a function without this mechanism
        # could be added
        self.collision = RigidBody.CollisionMesh( self.node, filepath, scale3d,
                                                  ( self.framecom.inv() * Frame.Frame(offset) ).offset(),
                                                  name_suffix )
        return self.collision

    def addVisualModel(self, filepath, scale3d=[1,1,1], offset=[0,0,0,0,0,0,1], name_suffix='', color=[1,1,1,1]):
        ## adding a visual model to the rigid body with a relative offset
        # @warning the translation due to the center of mass offset is
        # automatically removed. If necessary a function without this mechanism
        # could be added
        self.visual = RigidBody.VisualModel( self.node, filepath, scale3d, 
                                             ( self.framecom.inv() * Frame.Frame(offset) ).offset(), 
                                             name_suffix, color )
        return self.visual

    def addOffset(self, name, offset=[0,0,0,0,0,0,1], isMechanical=True):
        ## adding a relative offset to the rigid body (e.g. used as a joint location)

        # @warning the translation due to the center of mass offset is
        # automatically removed. If necessary a function without this mechanism
        # could be added
        return RigidBody.Offset( self.node, name, 
                                 ( self.framecom.inv() * Frame.Frame(offset) ).offset(), isMechanical )

    def addAbsoluteOffset(self, name, offset=[0,0,0,0,0,0,1], isMechanical=True):
        ## adding a offset given in absolute coordinates to the rigid body
        return RigidBody.Offset( self.node, name, (self.frame.inv()*Frame.Frame(offset)).offset(), isMechanical )

    def addMappedPoint(self, name, relativePosition=[0,0,0], isMechanical=True):
        ## adding a relative position to the rigid body

        # @warning the translation due to the center of mass offset is
        # automatically removed. If necessary a function without this mechanism
        # could be added
        frame = Frame.Frame(); frame.translation = relativePosition
        return RigidBody.MappedPoint( self.node, name, (self.framecom.inv()*frame).translation, isMechanical )

    def addAbsoluteMappedPoint(self, name, position=[0,0,0], isMechanical=True):
        ## adding a position given in absolute coordinates to the rigid body
        frame = Frame.Frame(); frame.translation = position
        return RigidBody.MappedPoint( self.node, name, (self.frame.inv()*frame).translation, isMechanical )

    def addMotor( self, forces=[0,0,0,0,0,0] ):
        ## adding a constant force/torque to the rigid body (that could be
        ## driven by a controller to simulate a motor)
        
        return self.node.createObject('ConstantForceField',
                                      template='Rigid3'+template_suffix, 
                                      name='motor', indices='0', forces=concat(forces))

    def setFixed(self, isFixed=True):
        """ Add/remove a fixed constraint for this rigid, also set speed to 0
        """
        if isFixed and self.fixedConstraint is None:
            self.fixedConstraint = self.node.createObject("FixedConstraint", warning=False, name="fixedConstraint")
            self.dofs.velocity=[0,0,0,0,0,0]
            self.fixedConstraint.init()
        elif not isFixed and not self.fixedConstraint is None:
            self.node.removeObject(self.fixedConstraint)
            self.fixedConstraint=None

    class CollisionMesh(object):

        def __init__(self, node, filepath, scale3d, offset, name_suffix=''):
            self.node = node.createChild( "collision"+name_suffix )  # node
            r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi
            self.loader = SofaPython.Tools.meshLoader(self.node, filename=filepath, name='loader', 
                                                      scale3d=concat(scale3d), translation=concat(offset[:3]) , 
                                                      rotation=r.tolist(), triangulate=True)
            self.topology = self.node.createObject('MeshTopology', name='topology', src="@loader" )
            self.dofs = self.node.createObject('MechanicalObject', name='dofs', template="Vec3"+template_suffix )
            self.triangles = self.node.createObject('TriangleModel', name='model')
            self.mapping = self.node.createObject('RigidMapping', name="mapping")
            self.normals = None
            self.visual = None

        def addNormals(self, invert=False):
            ## add a component to compute mesh normals at each timestep
            self.normals = self.node.createObject("NormalsFromPoints", template='Vec3'+template_suffix, 
                                                  name="normalsFromPoints", position='@'+self.dofs.name+'.position',
                                                  triangles='@'+self.topology.name+'.triangles', 
                                                  quads='@'+self.topology.name+'.quads', invertNormals=invert )

        def addVisualModel(self, color=[1,1,1,1]):
            ## add a visual model identical to the collision model
            self.visual = RigidBody.CollisionMesh.VisualModel( self.node, color=color )
            return self.visual

        class VisualModel(object):
            def __init__(self, node, color=[1,1,1,1]):
                global idxVisualModel
                self.node = node.createChild( "visual" )  # node
                # todo improve normal updates by using the Rigid Transform rather than by doing cross product
                # enforcing mesh loading in VisualModel to have correct texture coordinates
                self.model = self.node.createObject('VisualModel', name="model"+str(idxVisualModel), 
                                                    useNormals=False, updateNormals=True, 
                                                    fileMesh="@../loader.filename", color=concat(color) )
                
                self.mapping = self.node.createObject('IdentityMapping', name="mapping")
                idxVisualModel+=1

            @property
            def visual(self):
                # harmonize with Flexible.API.Deformable
                return self.model 
                

    class VisualModel(object):
        def __init__(self, node, filepath, scale3d, offset, name_suffix='', color=[1,1,1,1]):
            global idxVisualModel
            
            self.node = node.createChild( "visual"+name_suffix )  # node
            r = Quaternion.to_euler(offset[3:])  * 180.0 / math.pi

            self.model = self.node.createObject('VisualModel', name="visual"+str(idxVisualModel), fileMesh=filepath,
                                                scale3d=concat(scale3d), translation=concat(offset[:3]) , 
                                                rotation=r.tolist(),
                                                color=concat(color) )
            self.mapping = self.node.createObject('RigidMapping', name="mapping")
            idxVisualModel+=1


        @property
        def visual(self):
            # harmonize with Flexible.API.Deformable
            return self.model 

        
            
    class Offset(object):
        def __init__(self, node, name, offset, isMechanical=True):
            ## @param isMechanical:
            ##     will the Offset be used for mechanics? And then propagate forces or mass ?
            ##     Or will it be used only as a passive measure, visualization...?
            self.node = node.createChild( name )
            self.frame = Frame.Frame( offset ) # store the offset, relative position to its reference
            self.dofs = self.node.createObject('MechanicalObject', template="Rigid3"+template_suffix, name='dofs')
            self.mapping = self.node.createObject('AssembledRigidRigidMapping', 
                                                  name="mapping", source = '0 '+str(self.frame), 
                                                  geometricStiffness=geometric_stiffness)
            self.mapping.mapForces = isMechanical
            self.mapping.mapConstraints = isMechanical
            self.mapping.mapMasses = isMechanical

        def addOffset(self, name, offset=[0,0,0,0,0,0,1], isMechanical=True):
            ## adding a relative offset to the offset
            return RigidBody.Offset( self.node, name, offset, isMechanical )

        def addAbsoluteOffset(self, name, offset=[0,0,0,0,0,0,1], isMechanical=True):
            ## adding a offset given in absolute coordinates to the offset
            return RigidBody.Offset( self.node, name, 
                                     (Frame.Frame(offset) * self.frame.inv()).offset(), isMechanical )

        def applyTransform(self, transform=[0,0,0,0,0,0,1]):
            ## apply a rigid transform to the offset in its local frame
            self.frame = self.frame * Frame.Frame( transform )
            self.mapping.source = '0 '+str(self.frame)

        def addCollisionMesh(self, filepath, scale3d=[1, 1, 1], offset=[0, 0, 0, 0, 0, 0, 1], name_suffix=''):
            ## adding a collision mesh to the rigid body with a relative offset
            self.collision = RigidBody.CollisionMesh(self.node, filepath, scale3d, offset, name_suffix)
            return self.collision

        def addVisualModel(self, filepath, scale3d=[1, 1, 1], offset=[0, 0, 0, 0, 0, 0, 1], name_suffix='', color=[1,1,1,1]):
            ## adding a visual model to the rigid body with a relative offset
            self.visual = RigidBody.VisualModel(self.node, filepath, scale3d, offset, name_suffix, color)
            return self.visual

        def addMotor( self, forces=[0,0,0,0,0,0] ):
            ## adding a constant force/torque at the offset location (that could
            ## be driven by a controller to simulate a motor)
            return self.node.createObject('ConstantForceField', 
                                          template='Rigid3'+template_suffix, name='motor', indices='0', 
                                          forces=concat(forces))

        def addMappedPoint(self, name, relativePosition=[0,0,0], isMechanical=True):
            ## adding a relative position to the rigid body
            return RigidBody.MappedPoint( self.node, name, relativePosition, isMechanical )

        def addAbsoluteMappedPoint(self, name, position=[0,0,0], isMechanical=True):
            ## adding a position given in absolute coordinates to the rigid body
            frame = Frame.Frame(); frame.translation = position
            return RigidBody.MappedPoint( self.node, name, (frame * self.frame.inv()).translation, isMechanical )

        class MappedPoint(object):
            def __init__(self, node, name, position, isMechanical):
                self.node = node.createChild( name )
                self.dofs = self.node.createObject( 'MechanicalObject', name='dofs',
                                                    template="Vec3"+template_suffix, position=concat(position) )
                self.mapping = self.node.createObject('RigidMapping', name="mapping", 
                                                      geometricStiffness = geometric_stiffness)
                self.mapping.mapForces = isMechanical
                self.mapping.mapConstraints = isMechanical
                self.mapping.mapMasses = isMechanical

    class MappedPoint(object):
        def __init__(self, node, name, position, isMechanical):
            self.node = node.createChild( name )
            self.dofs = self.node.createObject( 'MechanicalObject', name='dofs', 
                                                template="Vec3"+template_suffix, position=concat(position) )
            
            self.mapping = self.node.createObject('RigidMapping', name="mapping", 
                                                  geometricStiffness = geometric_stiffness)
            self.mapping.mapForces = isMechanical
            self.mapping.mapConstraints = isMechanical
            self.mapping.mapMasses = isMechanical



class GenericRigidJoint(object):
    ## Generic kinematic joint between two Rigids

    def __init__(self, name, node1, node2, mask, compliance=0, index1=0, index2=0, isCompliance=True):
        self.node = node1.createChild( name )
        self.mask=mask
        self.dofs = self.node.createObject('MechanicalObject', template = 'Vec6'+template_suffix, 
                                           name = 'dofs', position = '0 0 0 0 0 0' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
        if not node2 is None:
            input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
            self.mapping = self.node.createObject('RigidJointMultiMapping', name = 'mapping', 
                                                  input = concat(input), output = '@dofs', 
                                                  pairs = str(index1)+" "+str(index2), 
                                                  geometricStiffness = geometric_stiffness)
            node2.addChild( self.node )
        else:
            self.mapping = self.node.createObject('RigidJointMapping', name = 'mapping', 
                                                  input = concat(input), output = '@dofs', 
                                                  pairs = str(index1)+" "+str(index2), 
                                                  geometricStiffness = geometric_stiffness)
            
        self.constraint = GenericRigidJoint.Constraint( self.node, mask, compliance, isCompliance )

    class Constraint(object):
        def __init__(self, node, mask, compliance, isCompliance):
            self.node = node.createChild( "constraint" )
            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
            self.mapping = self.node.createObject('MaskMapping',dofs=concat(mask))
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', 
                                                     compliance=compliance, isCompliance=isCompliance)
            # self.type = self.node.createObject('Stabilization') # do not add this component, it depends on the compliance value. The assembly will automatically add the best one

    class Limits(object):
        def __init__(self, node, masks, limits, compliance):
            self.node = node.createChild( "limits" )

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, 
                                               name='dofs', position=concat(position))
            self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance)
            # self.type = self.node.createObject('Stabilization')
            self.constraint = self.node.createObject('UnilateralConstraint')

    def addLimits( self, limits, compliance=0 ):
        # mtournier: docstring ffs
        
        ## limits is a list of limits in each unconstrained directions

        ## always lower and upper bounds per direction, so its size is two times
        ## the number of unconstrained directions (following the order
        ## txmin,txmax,tymin,tymax,tzmin,tzmax,rxmin,rxmax,rymin,rymax,rzmin,rzmax)

        ## limits can be set to None to represent unlimited directions
        
        usefulLimits = []

        l = 0
        limitMasks=[]
        for m in xrange(6):
            if self.mask[m] == 0: # limits can only set in unconstrained directions
                if limits[l] is not None and limits[l+1] is not None: # unconstrained direction with limits
                    limitMaskL = [0]*6
                    limitMaskU = [0]*6
                    limitMaskL[m] = 1 # lower bound
                    limitMaskU[m] = -1  # inverted upper bound
                    limitMasks.append( limitMaskL )
                    limitMasks.append( limitMaskU )
                    usefulLimits.append( limits[l] )
                    usefulLimits.append( -limits[l+1] ) # inverted upper bound
                l += 2
        if usefulLimits:
            return GenericRigidJoint.Limits( self.node, limitMasks, usefulLimits, compliance )
        else:
            return None

    def addDamper( self, damping ):
        return self.node.createObject( 'UniformVelocityDampingForceField', dampingCoefficient=damping )

    def addSpring( self, translation_stiffness=None, rotation_stiffness=None ):
        stiffnesses=[]
        for i in range(0,3):
            if not self.mask[i]:
                stiffnesses.append( float(translation_stiffness) )
            else:
                stiffnesses.append(0)
        for i in range(3,6):
            if not self.mask[i]:
                stiffnesses.append( float(rotation_stiffness) )
            else:
                stiffnesses.append(0)
        return self.node.createObject('DiagonalStiffness', stiffness=concat(stiffnesses))

    class VelocityController(object):
        def __init__(self, node, mask, velocities, compliance):
            self.node = node.createChild( "controller" )
            position = [0] * len(mask)
            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, 
                                               name='dofs', position=concat(position))
            
            self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', 
                                                     compliance=compliance, isCompliance=1)

            self.type = self.node.createObject('VelocityConstraintValue', velocities=concat(velocities))

        def setVelocities( self, velocities ):
            self.type.velocities = concat(velocities)

    class DefaultPositionController(object):
        """ Set the joint position to the target
        WARNING: for angular dof position, the value must be in ]-pi,pi]
        """
        def __init__(self, node, mask, target, compliance, isCompliance=False):
            self.node = node.createChild( "controller-mask" )

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
            self.node.createObject('MaskMapping', dofs=concat(mask))
            self.nodeTarget = self.node.createChild( "controller-target" )
            self.nodeTarget.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs')
            self.mapping = self.nodeTarget.createObject('DifferenceFromTargetMapping', targets=concat(target))
            self.compliance = self.nodeTarget.createObject('UniformCompliance', name='compliance', 
                                                           compliance=compliance, isCompliance=isCompliance)
            # self.type = self.nodeTarget.createObject('Stabilization')

        def setTarget( self, target ):
            self.mapping.targets = concat(target)

    # The PositionController can be redefined
    PositionController=DefaultPositionController

    def addGenericPositionController(self, target=None, compliance=0, mask=None, isCompliance=False):
        """ Add a controller to this joint.
        The target list must match mask list, il no target is specified, current position is used as a target.
        The mask list selects the controlled dof, if mask is None the joint natural dof are used.
        """
        if not mask is None:
            m=mask
        else:
            m = [ (1 - d) for d in self.mask ]
        if not target is None:
            t=target
        else:
            t=list()
            for i,v in enumerate(m):
                if v==1:
                    t.append(self.dofs.position[0][i])
        return GenericRigidJoint.PositionController(self.node, m, t, compliance, isCompliance)

    class ForceController(object):
        def __init__(self, node, mask, forces):
            self.node = node.createChild( "controller" )

            size = sum(x!=0 for x in mask)

            position = [0] * size
            points = numpy.arange(0,size,1)

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, 
                                               name='dofs', position=concat(position))
            
            self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
            self.force = self.node.createObject('ConstantForceField', template='Vec1'+template_suffix, 
                                                forces=concat(forces), indices=concat(points.tolist()) )

        def setForces( self, forces ):
            self.force.forces = concat(forces)


    class Resistance(object):
        def __init__(self, node, mask, threshold):
            self.node = node.createChild( "resistance" )
            position = [0] * len(mask)
            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
            self.mapping = self.node.createObject('MaskMapping', dofs=concat(mask))
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=0, isCompliance=1)
            self.type = self.node.createObject('VelocityConstraintValue', velocities=concat([0]) )
            self.constraint = self.node.createObject('ResistanceConstraint', threshold=threshold)



class CompleteRigidJoint(object):
    ## A complete kinematic joint between two Rigids
    # for advanced users!

    def __init__(self, name, node1, node2, compliances=[0,0,0,0,0,0], index1=0, index2=0):
        self.node = node1.createChild( name )
        self.dofs = self.node.createObject('MechanicalObject', template='Vec6'+template_suffix, name='dofs', position='0 0 0 0 0 0' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
        input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
        self.mapping = self.node.createObject('RigidJointMultiMapping',  name='mapping', input=concat(input), output='@dofs', pairs=str(index1)+" "+str(index2),
                                              geometricStiffness = geometric_stiffness)
        self.constraint = CompleteRigidJoint.Constraint( self.node, compliances ) # the constraint compliance cannot be in the same branch as eventual limits...
        node2.addChild( self.node )

    class Constraint(object):
        def __init__(self, node, compliances):
            self.node = node.createChild( "constraint" )
            self.dofs = self.node.createObject('MechanicalObject', template='Vec6'+template_suffix, name='dofs')
            self.mapping = self.node.createObject('IdentityMapping')
            self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances))
            # self.type = self.node.createObject('Stabilization')

    class Limits(object):
        def __init__(self, node, masks, limits, compliances):
            self.node = node.createChild( "limits" )

            set = []
            position = [0] * len(masks)
            offset = []

            for i in range(len(masks)):
                set = set + [0] + masks[i]
                offset.append(limits[i])

            self.dofs = self.node.createObject('MechanicalObject', template='Vec1'+template_suffix, name='dofs', position=concat(position))
            self.mapping = self.node.createObject('ProjectionMapping', set=concat(set), offset=concat(offset))
            self.compliance = self.node.createObject('DiagonalCompliance', name='compliance', compliance=concat(compliances))
            # self.type = self.node.createObject('Stabilization')
            self.constraint = self.node.createObject('UnilateralConstraint')

    def addLimits( self, masks, limits, compliances ):
            return CompleteRigidJoint.Limits( self.node, masks, limits, compliances )

    def addDamper( self, dampings=[0,0,0,0,0,0] ):
            return self.node.createObject( 'DiagonalVelocityDampingForceField', dampingCoefficients=dampings )

    def addSpring( self, stiffnesses=[0,0,0,0,0,0] ):
        return self.node.createObject('DiagonalStiffness', stiffness=concat(stiffnesses))


class HingeRigidJoint(GenericRigidJoint):
    ## Hinge/Revolute joint around the given axis (0->x, 1->y, 2->z)

    def __init__(self, axis, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        self.mask = [1] * 6; self.mask[3+axis]=0
        GenericRigidJoint.__init__(self, name, node1, node2, self.mask, compliance, index1, index2, isCompliance)

    def addLimits( self, lower, upper, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.Limits( self.node, [mask,(numpy.array(mask)*-1.).tolist()], [lower,-upper], compliance )

    def addSpring( self, stiffness ):
        return GenericRigidJoint.addSpring( self, rotation_stiffness=stiffness )

    def addPositionController( self, target, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.PositionController( self.node, mask, [target], compliance )

    def addVelocityController( self, velocity, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.VelocityController( self.node, mask, [velocity], compliance )

    def addForceController( self, force ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.ForceController( self.node, mask, [force] )

    def addResistance( self, threshold ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.Resistance( self.node, mask, threshold )


class SliderRigidJoint(GenericRigidJoint):
    ## Slider/Prismatic joint along the given axis (0->x, 1->y, 2->z)

    def __init__(self, axis, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        self.mask = [1] * 6; self.mask[axis]=0
        GenericRigidJoint.__init__(self, name, node1, node2, self.mask, compliance, index1, index2, isCompliance)

    def addLimits( self, lower, upper, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.Limits( self.node, [mask,(numpy.array(mask)*-1.).tolist()], [lower,-upper], compliance )

    def addSpring( self, stiffness ):
        return GenericRigidJoint.addSpring( self, translation_stiffness=stiffness)

    def addPositionController( self, target, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.PositionController( self.node, mask, [target], compliance )

    def addVelocityController( self, velocity, compliance=0 ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.VelocityController( self.node, mask, [velocity], compliance )

    def addForceController( self, force ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.ForceController( self.node, mask, [force] )

    def addResistance( self, threshold ):
        mask = [ (1 - d) for d in self.mask ]
        return GenericRigidJoint.Resistance( self.node, mask, threshold )


class CylindricalRigidJoint(GenericRigidJoint):
    ## Cylindrical joint along and around the given axis (0->x, 1->y, 2->z)

    def __init__(self, axis, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        mask = [1] * 6
        mask[axis]=0
        mask[3+axis]=0
        self.axis = axis
        GenericRigidJoint.__init__(self, name, node1, node2, mask, compliance, index1, index2, isCompliance)

    def addLimits( self, translation_lower, translation_upper, rotation_lower, rotation_upper, compliance=0 ):
        mask_t_l = [0]*6; mask_t_l[self.axis]=1;
        mask_t_u = [0]*6; mask_t_u[self.axis]=-1;
        mask_r_l = [0]*6; mask_r_l[3+self.axis]=1;
        mask_r_u = [0]*6; mask_r_u[3+self.axis]=-1;
        return GenericRigidJoint.Limits( self.node, [mask_t_l,mask_t_u,mask_r_l,mask_r_u], [translation_lower,-translation_upper,rotation_lower,-rotation_upper], compliance )

    def addSpring( self, translation_stiffness, rotation_stiffness ):
        return GenericRigidJoint.addSpring( self, translation_stiffness, rotation_stiffness )


class BallAndSocketRigidJoint(GenericRigidJoint):
    ## complete Ball and Socket / Spherical joint
    ## not the most efficient, but allowing rotation controllers

    def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2, isCompliance)

    # def addLimits( self, rotationX_lower, rotationX_upper, rotationY_lower, rotationY_upper, rotationZ_lower, rotationZ_upper, compliance=0 ):
        ## such limits make no sense for ball and socket. You could simulate it by mapping a point on one object and adding constraints/collisions on it
        # mask_x_l = [0]*6; mask_x_l[3]=1;
        # mask_x_u = [0]*6; mask_x_u[3]=-1;
        # mask_y_l = [0]*6; mask_y_l[4]=1;
        # mask_y_u = [0]*6; mask_y_u[4]=-1;
        # mask_z_l = [0]*6; mask_z_l[5]=1;
        # mask_z_u = [0]*6; mask_z_u[5]=-1;
        # return GenericRigidJoint.Limits( self.node, [mask_x_l,mask_x_u,mask_y_l,mask_y_u,mask_z_l,mask_z_u], [rotationX_lower,-rotationX_upper,rotationY_lower,-rotationY_upper,rotationZ_lower,-rotationZ_upper], compliance )

    def addSpring( self, stiffness ):
        return GenericRigidJoint.addSpring( self, rotation_stiffness=stiffness )

    def addPositionController( self, axis, target, compliance=0 ):
        """ control rotation around axis (0->x, 1->y, 2->z)
        """
        mask = [0]*6
        mask[3+axis]=1
        return GenericRigidJoint.PositionController( self.node, mask, [target], compliance )


class SimpleBallAndSocketRigidJoint(GenericRigidJoint):
    ## Ball and Socket / Spherical joint, much efficient than BallAndSocketRigidJoint
    ## but that cannot offer rotation controllers, neither spring and damping (would require a identitymapping level not to mix stiffness and constraint)

    def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        # GenericRigidJoint.__init__(self, name, node1, node2, [1,1,1,0,0,0], compliance, index1, index2) # GenericRigidJoint can work but is way overkill
        self.node = node1.createChild( name )
        self.dofs = self.node.createObject('MechanicalObject', template = 'Vec3'+template_suffix, name = 'dofs', position = '0 0 0' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
        input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
        self.mapping = self.node.createObject('DifferenceMultiMapping', name = 'mapping', input = concat(input), output = '@dofs', pairs = str(index1)+" "+str(index2))
        self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance, isCompliance=isCompliance)
        node2.addChild( self.node )



class PlanarRigidJoint(GenericRigidJoint):
    ## Planar joint for the given axis as plane normal (0->x, 1->y, 2->z)

    def __init__(self, normal, name, node1, node2, compliance=0, index1=0, index2=0, isCompliance=True ):
        self.normal = normal
        mask = [1]*6; mask[(normal+1)%3]=0; mask[(normal+2)%3]=0
        GenericRigidJoint.__init__(self, name, node1, node2, mask, compliance,index1, index2, isCompliance)

    def addLimits( self, translation1_lower, translation1_upper, translation2_lower, translation2_upper, compliance=0 ):
        axis1 = (self.normal+1)%3; axis2 = (self.normal+2)%3
        if axis1 > axis2 :
            axis1, axis2 = axis2, axis1
        mask_t1_l = [0]*6; mask_t1_l[axis1]=1;
        mask_t1_u = [0]*6; mask_t1_u[axis1]=-1;
        mask_t2_l = [0]*6; mask_t2_l[axis2]=1;
        mask_t2_u = [0]*6; mask_t2_u[axis2]=-1;
        return GenericRigidJoint.Limits( self.node, [mask_t1_l,mask_t1_u,mask_t2_l,mask_t2_u], [translation1_lower,-translation1_upper,translation2_lower,-translation2_upper], compliance )

    def addSpring( self, stiffness1, stiffness2 ):
        axis1 = (self.normal+1)%3; axis2 = (self.normal+2)%3
        if axis1 > axis2 :
            axis1, axis2 = axis2, axis1
        mask = [0]*6; mask[axis1]=stiffness1; mask[axis2]=stiffness2
        return self.node.createObject('DiagonalStiffness', stiffness=concat(mask))


#class GimbalRigidJoint(GenericRigidJoint):
#    ## Gimbal/Universal joint
## TODO it cannot be simulated a unique joint, but with 2 hinges with a third body in between


class FixedRigidJoint(GenericRigidJoint):
    ## Fixed joint

    def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0 ):
        self.node = node1.createChild( name )
        self.dofs = self.node.createObject('MechanicalObject', template = 'Vec6'+template_suffix, name = 'dofs', position = '0 0 0 0 0 0' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
        input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
        self.mapping = self.node.createObject('RigidJointMultiMapping', name = 'mapping', input = concat(input), output = '@dofs', pairs = str(index1)+" "+str(index2),
                                              geometricStiffness = geometric_stiffness)
        self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance)
        node2.addChild( self.node )

class DistanceRigidJoint(object):
    ## keep Distance between two rigid frames

    def __init__(self, name, node1, node2, compliance=0, index1=0, index2=0, rest_lenght=-1 ):
        self.node = node1.createChild( name )
        self.dofs = self.node.createObject('MechanicalObject', template='Rigid3'+template_suffix, name='dofs' )
        input = [] # @internal
        input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
        input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
        self.mapping = self.node.createObject('SubsetMultiMapping', name='mapping', input = concat(input), output = '@dofs', indexPairs="0 "+str(index1)+" 1 "+str(index2) )
        self.constraint = DistanceRigidJoint.Constraint(self.node, compliance, rest_lenght)
        node2.addChild( self.node )


    class Constraint(object):
        def __init__(self, node, compliance, rest_length ):
            self.node = node.createChild( 'constraint' )
            self.dofs = self.node.createObject('MechanicalObject', template = 'Vec1'+template_suffix, name = 'dofs', position = '0' )
            self.topology = self.node.createObject('EdgeSetTopologyContainer', edges="0 1" )
            self.mapping = self.node.createObject('DistanceMapping',  name='mapping', restLengths=(rest_length if rest_length>0 else "" ) )
            self.compliance = self.node.createObject('UniformCompliance', name='compliance', compliance=compliance)
            # self.type = self.node.createObject('Stabilization')

class RigidJointSpring(object):
    ## A 6D spring between two Rigids

    def __init__(self, name, node1, node2, stiffnesses=[0,0,0,0,0,0], index1=0, index2=0):
            self.node = node1.createChild( name )
            self.dofs = self.node.createObject('MechanicalObject', template = 'Vec6'+template_suffix, name = 'dofs', position = '0 0 0 0 0 0' )
            input = [] # @internal
            input.append( '@' + Tools.node_path_rel(self.node,node1) + '/dofs' )
            input.append( '@' + Tools.node_path_rel(self.node,node2) + '/dofs' )
            
            self.mapping = self.node.createObject('RigidJointMultiMapping',  name = 'mapping', input = concat(input), output = '@dofs', pairs = str(index1)+" "+str(index2),
                                                  geometricStiffness = geometric_stiffness)
            self.forcefield = self.node.createObject('DiagonalStiffness', name='ff', stiffness=concat(stiffnesses))
            node2.addChild( self.node )

## @TODO handle joints with diagonalcompliance / diagonaldamper...
## @TODO add mappings for more complex joints (eg with coupled dofs ie skrew, winch...)
## @TODO add a 6D Rigid Spring with a full stiffness matrix (not only diagonal)

