import bpy
import bpy_extras.object_utils
import bmesh
import mathutils
import numpy as np
from ..cAPI import SimulationAPI
from . import createTransform
from . import createMeshBVH
from . import createCurvesBVH
from . import getPositionArray
from . import pointBVHProximity
from ..properties import object_properties

def createBindConstraintObject(name, sim_obj_1, sim_obj_2, indices1, indices2, binding_positions1, binding_positions2, weights1, weights2, override_rest_length, rest_length):
    n_points = int(np.array(binding_positions1).size / 3)
    edge_mesh = bpy.data.meshes.new(name + "Mesh")
    edges = [(i, i + n_points) for i in range(n_points)]
    edge_mesh.from_pydata(np.concatenate((binding_positions1, binding_positions2)), edges, [])

    obj_new = bpy.data.objects.new(name, edge_mesh)
    obj_new.hi_phy.is_active = True
    obj_new.hi_phy.object_type = "CONSTRAINT"
    # Use the original object. We will evaluate it when we need it
    obj_new.hi_phy.constraint_object0 = sim_obj_1.original
    obj_new.hi_phy.constraint_object1 = sim_obj_2.original
    obj_new.hi_phy.constraint_type = SimulationAPI.ConstraintType.BindingConstraint
    obj_new.hi_phy.solver = sim_obj_1.hi_phy.solver
    object_properties.updt_bool_obj_type(obj_new.hi_phy)
    bpy.context.collection.objects.link(obj_new)

    # Do not create multiple attributes at the same time
    # It will make the previous handle invalid
    index0_0_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices0_0", 'INT', 'EDGE')
    index0_0_attr.data.foreach_set('value', indices1[0]);

    index0_1_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices0_1", 'INT', 'EDGE')
    index0_1_attr.data.foreach_set('value', indices1[1]);

    index0_2_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices0_2", 'INT', 'EDGE')
    index0_2_attr.data.foreach_set('value', indices1[2]);

    index1_0_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices1_0", 'INT', 'EDGE')
    index1_0_attr.data.foreach_set('value', indices2[0]);

    index1_1_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices1_1", 'INT', 'EDGE')
    index1_1_attr.data.foreach_set('value', indices2[1]);

    index1_2_attr = edge_mesh.attributes.new("hi_phy_constraint_point_indices1_2", 'INT', 'EDGE')
    index1_2_attr.data.foreach_set('value', indices2[2]);

    positions0_attr = edge_mesh.attributes.new("hi_phy_constraint_point_positions_0", 'FLOAT_VECTOR', 'EDGE')
    positions0_attr.data.foreach_set('vector', np.ravel(np.array(binding_positions1)));

    positions1_attr = edge_mesh.attributes.new("hi_phy_constraint_point_positions_1", 'FLOAT_VECTOR', 'EDGE')
    positions1_attr.data.foreach_set('vector', np.ravel(np.array(binding_positions2)));

    weight0_attr = edge_mesh.attributes.new("hi_phy_constraint_weight0", 'FLOAT_VECTOR', 'EDGE')
    weight0_attr.data.foreach_set('vector', np.ravel(np.array(weights1)));

    weight1_attr = edge_mesh.attributes.new("hi_phy_constraint_weight1", 'FLOAT_VECTOR', 'EDGE')
    weight1_attr.data.foreach_set('vector', np.ravel(np.array(weights2)));

    # note the rest length is in world space
    rest_length = [np.linalg.norm(np.array(binding_positions1[p]) - np.array(binding_positions2[p])) for p in range(n_points)] if not override_rest_length else [rest_length] * n_points
    rest_length_attr = edge_mesh.attributes.new("hi_phy_constraint_rest_length", 'FLOAT', 'EDGE')
    rest_length_attr.data.foreach_set('value', rest_length);

    edge_mesh.update()
    return edge_mesh

def createMeshMeshWithEdge(sim_obj_1, sim_obj_2, edges, binding_tolerance, name, override_rest_length, rest_length, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj_1.name + " is not a triangle mesh")
        return False
    if (not sim_obj_2.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj_2.name + " is not a triangle mesh")
        return False

    # build both BVH in world's space, as radius is in world space unit
    sim_obj_1_bvh = createMeshBVH(sim_obj_1, sim_obj_1.matrix_world, None, operator)
    if (sim_obj_1.name != sim_obj_2.name):
        sim_obj_2_bvh = createMeshBVH(sim_obj_2, sim_obj_2.matrix_world, None, operator)
    else:
        sim_obj_2_bvh = sim_obj_1_bvh

    if (not sim_obj_1_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    if (not sim_obj_2_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    mesh = edges.data
    bm = bmesh.new()
    bm.from_mesh(mesh)

    world_matrix = edges.matrix_world;
    end1 = [e.verts[0].co for e in bm.edges]
    end2 = [e.verts[1].co for e in bm.edges]

    n_points = len(end1)
    end1_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end1_p, end1)
    end2_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end2_p, end2)

    # Copy transforms
    sim_obj_1_transform = createTransform(sim_obj_1.matrix_world)
    sim_obj_2_transform = createTransform(sim_obj_2.matrix_world)
    edges_transform = createTransform(edges.matrix_world)
    # We need to bind each end to each object seperately
    found_proximity00 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh00 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids00 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh00 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord00 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end1_p, sim_obj_1_bvh, found_proximity00, bind_points_on_mesh00, bind_face_ids00,
                                                bind_barycentric_weights_on_mesh00, bind_local_coord00, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity01 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh01 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids01 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh01 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord01 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end1_p, sim_obj_2_bvh, found_proximity01, bind_points_on_mesh01, bind_face_ids01,
                                                bind_barycentric_weights_on_mesh01, bind_local_coord01, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity10 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh10 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids10 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh10 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord10 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end2_p, sim_obj_1_bvh, found_proximity10, bind_points_on_mesh10, bind_face_ids10,
                                                bind_barycentric_weights_on_mesh10, bind_local_coord10, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    found_proximity11 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh11 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids11 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh11 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord11 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end2_p, sim_obj_2_bvh, found_proximity11, bind_points_on_mesh11, bind_face_ids11,
                                                bind_barycentric_weights_on_mesh11, bind_local_coord11, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    # Now we combine the two ends to make up
    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    weights_1 = np.zeros((n_points, 3), dtype=np.float32)
    weights_2 = np.zeros((n_points, 3), dtype=np.float32)
    indices0_0 = [0] * n_points
    indices0_1 = [0] * n_points
    indices0_2 = [0] * n_points
    indices1_0 = [0] * n_points
    indices1_1 = [0] * n_points
    indices1_2 = [0] * n_points
    for i in range(n_points):
        # First check only one end is bond by each
        if (found_proximity00[i] and found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity00[i] and not found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " didn't bind on either side")
                return False
        if (found_proximity10[i] and found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity10[i] and not found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " didn't bind on either side")
                return False
        if found_proximity00[i]:
            weights_1[i] = (bind_barycentric_weights_on_mesh00[i][0],
                            bind_barycentric_weights_on_mesh00[i][1],
                            bind_barycentric_weights_on_mesh00[i][2])
            face = sim_obj_1.data.polygons[bind_face_ids00[i]]
            indices0_0[i] = face.vertices[0]
            indices0_1[i] = face.vertices[1]
            indices0_2[i] = face.vertices[2]
            vs_world_1[i] = edges.matrix_world @ end1[i];
        if found_proximity01[i]:
            weights_2[i] = (bind_barycentric_weights_on_mesh01[i][0],
                            bind_barycentric_weights_on_mesh01[i][1],
                            bind_barycentric_weights_on_mesh01[i][2])
            face = sim_obj_2.data.polygons[bind_face_ids01[i]]
            indices1_0[i] = face.vertices[0]
            indices1_1[i] = face.vertices[1]
            indices1_2[i] = face.vertices[2]
            vs_world_2[i] = edges.matrix_world @ end1[i];
        if found_proximity10[i]:
            weights_1[i] = (bind_barycentric_weights_on_mesh10[i][0],
                            bind_barycentric_weights_on_mesh10[i][1],
                            bind_barycentric_weights_on_mesh10[i][2])
            face = sim_obj_1.data.polygons[bind_face_ids10[i]]
            indices0_0[i] = face.vertices[0]
            indices0_1[i] = face.vertices[1]
            indices0_2[i] = face.vertices[2]
            vs_world_1[i] = edges.matrix_world @ end2[i];
        if found_proximity11[i]:
            weights_2[i] = (bind_barycentric_weights_on_mesh11[i][0],
                            bind_barycentric_weights_on_mesh11[i][1],
                            bind_barycentric_weights_on_mesh11[i][2])
            face = sim_obj_2.data.polygons[bind_face_ids11[i]]
            indices1_0[i] = face.vertices[0]
            indices1_1[i] = face.vertices[1]
            indices1_2[i] = face.vertices[2]
            vs_world_2[i] = edges.matrix_world @ end2[i];

    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weights_1, weights_2, override_rest_length, rest_length)
    return True

def createCurvesCurvesWithEdge(sim_obj_1, sim_obj_2, edges, binding_tolerance, name, override_rest_length, rest_length, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding object " + sim_obj_1.name + " is not a curves object")
        return False
    if (not sim_obj_2.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding object " + sim_obj_2.name + " is not a curves object")
        return False

    # build both BVH in world's space, as radius is in world space unit
    sim_obj_1_bvh = createCurvesBVH(sim_obj_1, sim_obj_1.matrix_world, None, operator)
    if (sim_obj_1.name != sim_obj_2.name):
        sim_obj_2_bvh = createCurvesBVH(sim_obj_2, sim_obj_2.matrix_world, None, operator)
    else:
        sim_obj_2_bvh = sim_obj_1_bvh

    if (not sim_obj_1_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    if (not sim_obj_2_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    mesh = edges.data
    bm = bmesh.new()
    bm.from_mesh(mesh)

    world_matrix = edges.matrix_world;
    end1 = [e.verts[0].co for e in bm.edges]
    end2 = [e.verts[1].co for e in bm.edges]

    n_points = len(end1)
    end1_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end1_p, end1)
    end2_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end2_p, end2)

    # Copy transforms
    sim_obj_1_transform = createTransform(sim_obj_1.matrix_world)
    sim_obj_2_transform = createTransform(sim_obj_2.matrix_world)
    edges_transform = createTransform(edges.matrix_world)

    # We need to bind each end to each object seperately
    found_proximity00 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves00 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids00 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge00 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end1_p, sim_obj_1_bvh, found_proximity00, bind_points_on_curves00, bind_edge_ids00,
                                                  bind_weights_on_edge00, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity01 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves01 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids01 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge01 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end1_p, sim_obj_2_bvh, found_proximity01, bind_points_on_curves01, bind_edge_ids01,
                                                  bind_weights_on_edge01, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    # We need to bind each end to each object seperately
    found_proximity10 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves10 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids10 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge10 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end2_p, sim_obj_1_bvh, found_proximity10, bind_points_on_curves10, bind_edge_ids10,
                                                  bind_weights_on_edge10, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    # We need to bind each end to each object seperately
    found_proximity11 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves11 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids11 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge11 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end2_p, sim_obj_2_bvh, found_proximity11, bind_points_on_curves11, bind_edge_ids11,
                                                  bind_weights_on_edge11, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    # Now we combine the two ends to make up
    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    weights_1 = np.zeros((n_points, 3), dtype=np.float32)
    weights_2 = np.zeros((n_points, 3), dtype=np.float32)
    indices0_0 = [0] * n_points
    indices0_1 = [0] * n_points
    indices0_2 = [0] * n_points
    indices1_0 = [0] * n_points
    indices1_1 = [0] * n_points
    indices1_2 = [0] * n_points
    for i in range(n_points):
        # First check only one end is bond by each
        if (found_proximity00[i] and found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity00[i] and not found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " didn't bind on either side")
                return False
        if (found_proximity10[i] and found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity10[i] and not found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " didn't bind on either side")
                return False
        if found_proximity00[i]:
            weights_1[i] = (bind_weights_on_edge00[i],
                            1 - bind_weights_on_edge00[i],
                            0)
            indices0_0[i] = sim_obj_1_bvh.GetCVFromEdge(bind_edge_ids00[i])
            indices0_1[i] = indices0_0[i] + 1
            indices0_2[i] = indices0_0[i]
            vs_world_1[i] = edges.matrix_world @ end1[i];
        if found_proximity01[i]:
            weights_2[i] = (bind_weights_on_edge01[i],
                            1 - bind_weights_on_edge01[i],
                            0)
            indices1_0[i] = sim_obj_2_bvh.GetCVFromEdge(bind_edge_ids01[i])
            indices1_1[i] = indices1_0[i] + 1
            indices1_2[i] = indices1_0[i]
            vs_world_2[i] = edges.matrix_world @ end1[i];
        if found_proximity10[i]:
            weights_1[i] = (bind_weights_on_edge10[i],
                            1 - bind_weights_on_edge10[i],
                            0)
            indices0_0[i] = sim_obj_1_bvh.GetCVFromEdge(bind_edge_ids10[i])
            indices0_1[i] = indices0_0[i] + 1
            indices0_2[i] = indices0_0[i]
            vs_world_1[i] = edges.matrix_world @ end2[i];
        if found_proximity11[i]:
            weights_2[i] = (bind_weights_on_edge11[i],
                            1 - bind_weights_on_edge11[i],
                            0)
            indices1_0[i] = sim_obj_2_bvh.GetCVFromEdge(bind_edge_ids11[i])
            indices1_1[i] = indices1_0[i] + 1
            indices1_2[i] = indices1_0[i]
            vs_world_2[i] = edges.matrix_world @ end2[i];

    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weights_1, weights_2, override_rest_length, rest_length)
    return True

def createMeshCurvesWithEdge(sim_obj_1, sim_obj_2, edges, binding_tolerance, name, override_rest_length, rest_length, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding object " + sim_obj_1.name + " is not a mesh object")
        return False
    if (not sim_obj_2.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding object " + sim_obj_2.name + " is not a curves object")
        return False

    # build both BVH in world's space, as radius is in world space unit
    sim_obj_1_bvh = createMeshBVH(sim_obj_1, sim_obj_1.matrix_world, None, operator)
    sim_obj_2_bvh = createCurvesBVH(sim_obj_2, sim_obj_2.matrix_world, None, operator)

    if (not sim_obj_1_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    if (not sim_obj_2_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    mesh = edges.data
    bm = bmesh.new()
    bm.from_mesh(mesh)

    world_matrix = edges.matrix_world;
    end1 = [e.verts[0].co for e in bm.edges]
    end2 = [e.verts[1].co for e in bm.edges]

    n_points = len(end1)
    end1_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end1_p, end1)
    end2_p = SimulationAPI.CreateVector3Array(n_points)
    SimulationAPI.CopyVector3Array(end2_p, end2)

    # Copy transforms
    sim_obj_1_transform = createTransform(sim_obj_1.matrix_world)
    sim_obj_2_transform = createTransform(sim_obj_2.matrix_world)
    edges_transform = createTransform(edges.matrix_world)

    # We need to bind each end to each object seperately
    found_proximity00 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh00 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids00 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh00 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord00 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end1_p, sim_obj_1_bvh, found_proximity00, bind_points_on_mesh00, bind_face_ids00,
                                                bind_barycentric_weights_on_mesh00, bind_local_coord00, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity01 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves01 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids01 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge01 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end1_p, sim_obj_2_bvh, found_proximity01, bind_points_on_curves01, bind_edge_ids01,
                                                  bind_weights_on_edge01, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity10 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_mesh10 = SimulationAPI.CreateVector3Array(n_points)
    bind_face_ids10 = SimulationAPI.CreateIntArray(n_points)
    bind_barycentric_weights_on_mesh10 = SimulationAPI.CreateVector3Array(n_points)
    bind_local_coord10 = SimulationAPI.CreateVector3Array(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToMesh(end2_p, sim_obj_1_bvh, found_proximity10, bind_points_on_mesh10, bind_face_ids10,
                                                bind_barycentric_weights_on_mesh10, bind_local_coord10, edges_transform, sim_obj_1_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False
    # We need to bind each end to each object seperately
    found_proximity11 = SimulationAPI.CreateBoolArray(n_points)
    bind_points_on_curves11 = SimulationAPI.CreateVector3Array(n_points)
    bind_edge_ids11 = SimulationAPI.CreateIntArray(n_points)
    bind_weights_on_edge11 = SimulationAPI.CreateFloatArray(n_points)
    # Binding
    if not SimulationAPI.Utils.BindPointsToCurves(end2_p, sim_obj_2_bvh, found_proximity11, bind_points_on_curves11, bind_edge_ids11,
                                                  bind_weights_on_edge11, edges_transform, sim_obj_2_transform, binding_tolerance):
        if operator:
            operator.report({"ERROR"}, "Binding has failed")
            return False

    # Now we combine the two ends to make up
    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    weights_1 = np.zeros((n_points, 3), dtype=np.float32)
    weights_2 = np.zeros((n_points, 3), dtype=np.float32)
    indices0_0 = [0] * n_points
    indices0_1 = [0] * n_points
    indices0_2 = [0] * n_points
    indices1_0 = [0] * n_points
    indices1_1 = [0] * n_points
    indices1_2 = [0] * n_points
    for i in range(n_points):
        # First check only one end is bond by each
        if (found_proximity00[i] and found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity00[i] and not found_proximity01[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end1[i]) + " didn't bind on either side")
                return False
        if (found_proximity10[i] and found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " is bounded with both end, are they too close?")
                return False
        if (not found_proximity10[i] and not found_proximity11[i]):
            if operator:
                operator.report({"ERROR"}, "Point " + str(edges.matrix_world @ end2[i]) + " didn't bind on either side")
                return False
        if found_proximity00[i]:
            weights_1[i] = (bind_barycentric_weights_on_mesh00[i][0],
                            bind_barycentric_weights_on_mesh00[i][1],
                            bind_barycentric_weights_on_mesh00[i][2])
            face = sim_obj_1.data.polygons[bind_face_ids00[i]]
            indices0_0[i] = face.vertices[0]
            indices0_1[i] = face.vertices[1]
            indices0_2[i] = face.vertices[2]
            vs_world_1[i] = edges.matrix_world @ end1[i];
        if found_proximity01[i]:
            weights_2[i] = (bind_weights_on_edge01[i],
                            1 - bind_weights_on_edge01[i],
                            0)
            indices1_0[i] = sim_obj_2_bvh.GetCVFromEdge(bind_edge_ids01[i])
            indices1_1[i] = indices1_0[i] + 1
            indices1_2[i] = indices1_0[i]
            vs_world_2[i] = edges.matrix_world @ end1[i];
        if found_proximity10[i]:
            weights_1[i] = (bind_barycentric_weights_on_mesh10[i][0],
                            bind_barycentric_weights_on_mesh10[i][1],
                            bind_barycentric_weights_on_mesh10[i][2])
            face = sim_obj_1.data.polygons[bind_face_ids10[i]]
            indices0_0[i] = face.vertices[0]
            indices0_1[i] = face.vertices[1]
            indices0_2[i] = face.vertices[2]
            vs_world_1[i] = edges.matrix_world @ end2[i];
        if found_proximity11[i]:
            weights_2[i] = (bind_weights_on_edge11[i],
                            1 - bind_weights_on_edge11[i],
                            0)
            indices1_0[i] = sim_obj_2_bvh.GetCVFromEdge(bind_edge_ids11[i])
            indices1_1[i] = indices1_0[i] + 1
            indices1_2[i] = indices1_0[i]
            vs_world_2[i] = edges.matrix_world @ end2[i];

    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weights_1, weights_2, override_rest_length, rest_length)
    return True

def createMeshWithinWithEdge(sim_obj, edges, name, override_rest_length, rest_length, operator = None):
    if not sim_obj.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj.name + " is not a valid hi phy object")
        return False
    if (not sim_obj.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj.name + " is not a triangle mesh")
        return False
    bm = None
    # First is to find the stray edges
    mesh = sim_obj.data
    bm = bmesh.new()
    bm.from_mesh(mesh)
    if edges:
        bm.edges.ensure_lookup_table()
        edges_without_faces = [bm.edges[i] for i in edges if len(bm.edges[i].link_faces) == 0]
    else:
        edges_without_faces = [e for e in bm.edges if len(e.link_faces) == 0]
    if (len(edges_without_faces) == 0):
        bm.free()
        if operator:
            operator.report({"ERROR"}, "Mesh " + sim_obj.name + " does not have any stray edges for binding.")
        return False
    indices1 = [e.verts[0].index for e in edges_without_faces]
    indices2 = [e.verts[1].index for e in edges_without_faces]
    world_matrix = sim_obj.matrix_world;
    location1 = [world_matrix @ e.verts[0].co for e in edges_without_faces]
    location2 = [world_matrix @ e.verts[1].co for e in edges_without_faces]
    weights1 = [(1, 0, 0)] * len(edges_without_faces)
    weights2 = [(1, 0, 0)] * len(edges_without_faces)
    createBindConstraintObject(name, sim_obj, sim_obj,
                               [indices1, indices1, indices1],
                               [indices2, indices2, indices2],
                               location1, location2,
                               weights1, weights2, override_rest_length, rest_length)
    bm.free()
    return True

# we assume the object passed in has already been evaluated
def createMeshMesh(sim_obj_1, sim_obj_2, binding_radius, name, override_rest_length, rest_length,
                   sim_obj_1_selected_verts = None, sim_obj_2_selected_verts = None, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj_1.name + " is not a triangle mesh")
        return False
    if (not sim_obj_2.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj_2.name + " is not a triangle mesh")
        return False

    # build both BVH in world's space, as radius is in world space unit
    radius_1 = [binding_radius] * len(sim_obj_1.data.vertices)
    radius_2 = [binding_radius] * len(sim_obj_2.data.vertices)
    sim_obj_1_bvh = createMeshBVH(sim_obj_1, sim_obj_1.matrix_world, radius_1, operator)

    if (sim_obj_1.name != sim_obj_2.name):
        sim_obj_2_bvh = createMeshBVH(sim_obj_2, sim_obj_2.matrix_world, radius_2, operator)
    else:
        sim_obj_2_bvh = sim_obj_1_bvh

    if (not sim_obj_1_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    if (not sim_obj_2_bvh) :
        if operator:
            operator.report({"ERROR"}, "Failed to build BVH for " + sim_obj_1.name)
        return False

    points1 = SimulationAPI.CreateVector3Array()
    points2 = SimulationAPI.CreateVector3Array()
    verts1 = SimulationAPI.CreateInt3Array()
    verts2 = SimulationAPI.CreateInt3Array()
    barycentric_weights1 = SimulationAPI.CreateVector3Array()
    barycentric_weights2 = SimulationAPI.CreateVector3Array()
    rest_distance = SimulationAPI.CreateFloatArray()
    active_verts_1 = None
    active_verts_2 = None
    if sim_obj_1_selected_verts:
        active_verts_1 = SimulationAPI.CreateIntArray(len(sim_obj_1_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_1, sim_obj_1_selected_verts)
    if sim_obj_2_selected_verts:
        active_verts_2 = SimulationAPI.CreateIntArray(len(sim_obj_2_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_2, sim_obj_2_selected_verts)

    print(active_verts_1)
    print(active_verts_2)
    SimulationAPI.Utils.MeshMeshProximity(sim_obj_1_bvh, sim_obj_2_bvh,
                                          points1, points2,
                                          verts1, verts2,
                                          barycentric_weights1, barycentric_weights2,
                                          rest_distance,
                                          active_verts_1, active_verts_2);

    # bond_points_1 and 2 should have same points
    n_points = len(points1)
    if (n_points == 0):
        if operator:
            operator.report({"ERROR"}, "Nothing was bond")
            return False

    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_1, points1)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_2, points2)

    weights_1 = np.zeros((n_points, 3), dtype=np.float32)
    weights_2 = np.zeros((n_points, 3), dtype=np.float32)
    SimulationAPI.CopyVector3ArrayToPython(weights_1, barycentric_weights1)
    SimulationAPI.CopyVector3ArrayToPython(weights_2, barycentric_weights2)

    indices0_0 = [v[0] for v in verts1]
    indices0_1 = [v[1] for v in verts1]
    indices0_2 = [v[2] for v in verts1]
    indices1_0 = [v[0] for v in verts2]
    indices1_1 = [v[1] for v in verts2]
    indices1_2 = [v[2] for v in verts2]

    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weights_1, weights_2, override_rest_length, rest_length)
    return True

# we assume the object passed in has already been evaluated
def createCurvesCurves(sim_obj_1, sim_obj_2, binding_radius, name, override_rest_length, rest_length, curve_self_binding,
                       sim_obj_1_selected_verts = None, sim_obj_2_selected_verts = None, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding curves " + sim_obj_1.name + " is not a curves object")
        return False
    if (not sim_obj_2.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding curves " + sim_obj_2.name + " is not a curves object")
        return False

    # build both BVH in world space (radius is in world space)
    n_cvs1 = sim_obj_1.data.attributes.domain_size('POINT')
    n_cvs2 = sim_obj_2.data.attributes.domain_size('POINT')
    radius_1 = [binding_radius] * n_cvs1
    radius_2 = [binding_radius] * n_cvs2
    sim_obj_1_bvh = createCurvesBVH(sim_obj_1, sim_obj_1.matrix_world, radius_1, operator)
    if (not sim_obj_1_bvh):
        if operator:
            operator.report({"ERROR"}, "Failed to create BVH for " + sim_obj_1.name)
        return False
    if (sim_obj_1.name != sim_obj_2.name):
        sim_obj_2_bvh = createCurvesBVH(sim_obj_2, sim_obj_2.matrix_world, radius_2, operator)
    else:
        sim_obj_2_bvh = sim_obj_1_bvh
    if (not sim_obj_2_bvh):
        if operator:
            operator.report({"ERROR"}, "Failed to create BVH for " + sim_obj_2.name)
        return False
    points1 = SimulationAPI.CreateVector3Array()
    points2 = SimulationAPI.CreateVector3Array()
    cv1 = SimulationAPI.CreateIntArray()
    cv2 = SimulationAPI.CreateIntArray()
    k1 = SimulationAPI.CreateFloatArray()
    k2 = SimulationAPI.CreateFloatArray()
    rest_distance = SimulationAPI.CreateFloatArray()
    active_verts_1 = None
    active_verts_2 = None
    if sim_obj_1_selected_verts:
        active_verts_1 = SimulationAPI.CreateIntArray(len(sim_obj_1_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_1, sim_obj_1_selected_verts)
    if sim_obj_2_selected_verts:
        active_verts_2 = SimulationAPI.CreateIntArray(len(sim_obj_2_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_2, sim_obj_2_selected_verts)

    SimulationAPI.Utils.CurvesCurvesProximity(sim_obj_1_bvh, sim_obj_2_bvh,
                                              points1, points2,
                                              cv1, cv2, k1, k2,
                                              rest_distance, not curve_self_binding,
                                              active_verts_1, active_verts_2);

    # bond_points_1 and 2 should have same points
    n_points = len(points1)
    if (n_points == 0):
        if operator:
            operator.report({"ERROR"}, "Nothing was bond")
            return False
    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_1, points1)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_2, points2)

    indices0_0 = [cv for cv in cv1]
    indices0_1 = [cv + 1 for cv in cv1]
    indices0_2 = [cv for cv in cv1]
    indices1_0 = [cv for cv in cv2]
    indices1_1 = [cv + 1 for cv in cv2]
    indices1_2 = [cv for cv in cv2]
    weight0 = [w for k in k1 for w in [k, 1 - k, 0]]
    weight1 = [w for k in k2 for w in [k, 1 - k, 0]]
    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weight0, weight1, override_rest_length, rest_length)

    return True

# we assume the object passed in has already been evaluated
def createMeshCurve(sim_obj_1, sim_obj_2, binding_radius, name, override_rest_length, rest_length,
                    sim_obj_1_selected_verts = None, sim_obj_2_selected_verts = None, operator = None):
    if not sim_obj_1.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid hi phy object")
        return False
    if not sim_obj_2.hi_phy:
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid hi phy object")
        return False

    # get mesh topology
    if (not sim_obj_1.type == 'MESH') :
        if operator:
            operator.report({"ERROR"}, "Binding mesh " + sim_obj_1.name + " is not a triangle mesh")
        return False
    if (not sim_obj_2.type == 'CURVES') :
        if operator:
            operator.report({"ERROR"}, "Binding object " + sim_obj_2.name + " curves object")
        return False

    # Copy transforms
    # build both BVH in world space, we have to because radius is in world space unit
    radius_1 = [binding_radius] * sim_obj_1.data.attributes.domain_size('POINT')
    radius_2 = [binding_radius] * sim_obj_2.data.attributes.domain_size('POINT')
    sim_obj_1_bvh = createMeshBVH(sim_obj_1, sim_obj_1.matrix_world, radius_1, operator)
    if (not sim_obj_1_bvh):
        if operator:
            operator.report({"ERROR"}, "Failed to create BVH for " + sim_obj_1.name)
        return False
    sim_obj_2_bvh = createCurvesBVH(sim_obj_2, sim_obj_2.matrix_world, radius_2, operator)
    if (not sim_obj_2_bvh):
        if operator:
            operator.report({"ERROR"}, "Failed to create BVH for " + sim_obj_2.name)
        return False

    points1 = SimulationAPI.CreateVector3Array()
    points2 = SimulationAPI.CreateVector3Array()
    verts1 = SimulationAPI.CreateInt3Array()
    cv2 = SimulationAPI.CreateIntArray()
    barycentric_weights1 = SimulationAPI.CreateVector3Array()
    k2 = SimulationAPI.CreateFloatArray()
    rest_distance = SimulationAPI.CreateFloatArray()

    active_verts_1 = None
    active_verts_2 = None
    if sim_obj_1_selected_verts:
        active_verts_1 = SimulationAPI.CreateIntArray(len(sim_obj_1_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_1, sim_obj_1_selected_verts)
    if sim_obj_2_selected_verts:
        active_verts_2 = SimulationAPI.CreateIntArray(len(sim_obj_2_selected_verts))
        SimulationAPI.CopyIntArray(active_verts_2, sim_obj_2_selected_verts)

    SimulationAPI.Utils.MeshCurvesProximity(sim_obj_1_bvh, sim_obj_2_bvh,
                                            points1, points2,
                                            verts1, cv2,
                                            barycentric_weights1, k2,
                                            rest_distance,
                                            active_verts_1, active_verts_2);

    # bond_points_1 and 2 should have same points
    n_points = len(points1)
    if (n_points == 0):
        if operator:
            operator.report({"ERROR"}, "Nothing was bond")
            return False

    vs_world_1 = np.zeros((n_points, 3), dtype=np.float32)
    vs_world_2 = np.zeros((n_points, 3), dtype=np.float32)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_1, points1)
    SimulationAPI.CopyVector3ArrayToPython(vs_world_2, points2)

    indices0_0 = [v[0] for v in verts1]
    indices0_1 = [v[1] for v in verts1]
    indices0_2 = [v[2] for v in verts1]
    indices1_0 = [cv for cv in cv2]
    indices1_1 = [cv + 1 for cv in cv2]
    indices1_2 = [cv for cv in cv2]

    weights_1 = np.zeros((n_points, 3), dtype=np.float32)
    SimulationAPI.CopyVector3ArrayToPython(weights_1, barycentric_weights1)
    weights_2 = [w for k in k2 for w in [k, 1 - k, 0]]

    createBindConstraintObject(name, sim_obj_1, sim_obj_2,
                               [indices0_0, indices0_1, indices0_2],
                               [indices1_0, indices1_1, indices1_2],
                               vs_world_1, vs_world_2,
                               weights_1, weights_2, override_rest_length, rest_length)
    return True
