import bpy
import bpy_extras.object_utils
import mathutils
import numpy as np
from ..cAPI import SimulationAPI
from . import createTransform
from ..properties import object_properties

# we assume the verts are already part of the object
# and the object passed in has already been evaluated
def createJointConstraint(sim_obj_1, sim_obj_2, joint_location, constraint_order, constraint_direction, constraint_direction_space, name, operator = None):
    if not sim_obj_1.hi_phy or not sim_obj_1.hi_phy.is_active or not sim_obj_1.hi_phy.object_type == 'AFFINE_BODY':
        if operator:
            operator.report({"ERROR"}, sim_obj_1.name + " is not a valid affine body object")
        return False

    if sim_obj_2 and (not sim_obj_2.hi_phy or not sim_obj_2.hi_phy.is_active or not sim_obj_2.hi_phy.object_type == 'AFFINE_BODY'):
        if operator:
            operator.report({"ERROR"}, sim_obj_2.name + " is not a valid affine body object")
        return False

    # Binding
    pointcloud = bpy.data.meshes.new(name + "Mesh")
    n_points = 1
    # The bond points are in sim object space
    vs_world = [joint_location]

    pointcloud.from_pydata(vs_world, [], []) # Create a mesh with only verts

    obj_new = bpy.data.objects.new(name, pointcloud)
    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
    if (sim_obj_2) :
        obj_new.hi_phy.constraint_object1 = sim_obj_2.original
        obj_new.hi_phy.constraint_type = SimulationAPI.ConstraintType.BindingConstraint
    else:
        obj_new.hi_phy.constraint_object1 = sim_obj_1.original
        obj_new.hi_phy.constraint_type = SimulationAPI.ConstraintType.PointConstraint
        obj_new.hi_phy.constraintIsSoft = True;
        # If we are doing point constraints, we need convert the constraint direction from object
        if (constraint_direction_space ==  SimulationAPI.ConstraintDirectionSpace.ConstraintDirectionSpaceObject2):
            if operator:
                operator.report({"ERROR"}, "Single joint constraint can not use second object as constraint direction.")
            return False
        if (constraint_direction_space ==  SimulationAPI.ConstraintDirectionSpace.ConstraintDirectionSpaceObject1):
            # If we are doing point constraints, we need convert the constraint direction from object space to world space
            # Point constraint direction can not be computed in object space, it will cause drift from soft constraints.
            # However, we still allow user to define it in object space for convinience.
            # We will need to transform it to world space
            constraint_direction = sim_obj_1.matrix_world.to_3x3() @ mathutils.Vector((constraint_direction[0], constraint_direction[1], constraint_direction[2]));

    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
    # Set an invalid value so simulator can check it is only intended for affine body
    index0_0_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices0_0", 'INT', 'POINT')
    index0_0_attr.data.foreach_set('value', [-1]);

    index0_1_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices0_1", 'INT', 'POINT')
    index0_1_attr.data.foreach_set('value', [-1]);

    index0_2_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices0_2", 'INT', 'POINT')
    index0_2_attr.data.foreach_set('value', [-1]);

    # Use this to store face id
    index1_0_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices1_0", 'INT', 'POINT')
    index1_0_attr.data.foreach_set('value', [-1]);

    index1_1_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices1_1", 'INT', 'POINT')
    index1_1_attr.data.foreach_set('value', [-1]);

    index1_2_attr = pointcloud.attributes.new("hi_phy_constraint_point_indices1_2", 'INT', 'POINT')
    index1_2_attr.data.foreach_set('value', [-1]);

    weight0_attr = pointcloud.attributes.new("hi_phy_constraint_weight0", 'FLOAT_VECTOR', 'POINT')
    weight0_attr.data.foreach_set('vector', [0, 0, 0]);

    # Use this to store barycentric weights
    weight1_attr = pointcloud.attributes.new("hi_phy_constraint_weight1", 'FLOAT_VECTOR', 'POINT')
    weight1_attr.data.foreach_set('vector', [0, 0, 0]);

    # rest length is still zero (zero distance to the bond point)
    rest_length_attr = pointcloud.attributes.new("hi_phy_constraint_rest_length", 'FLOAT', 'POINT')
    rest_length_attr.data.foreach_set('value', [0]);

    local_coord_attr = pointcloud.attributes.new("hi_phy_constraint_local_coord", 'FLOAT_VECTOR', 'POINT')
    local_coord_attr.data.foreach_set('vector', [0, 0, 0]);

    # TODO: make it in a better space, it can have issues if we are far from origin to have joint location in world space
    positions0_attr = pointcloud.attributes.new("hi_phy_constraint_point_positions_0", 'FLOAT_VECTOR', 'POINT')
    positions0_attr.data.foreach_set('vector', joint_location);

    positions1_attr = pointcloud.attributes.new("hi_phy_constraint_point_positions_1", 'FLOAT_VECTOR', 'POINT')
    positions1_attr.data.foreach_set('vector', joint_location);

    order_attr = pointcloud.attributes.new("hi_phy_constraint_order", 'INT', 'POINT')
    order_attr.data.foreach_set('value', [constraint_order]);

    direction_space_attr = pointcloud.attributes.new("hi_phy_constraint_direction_space", 'INT', 'POINT')
    direction_space_attr.data.foreach_set('value', [constraint_direction_space]);

    direction_attr = pointcloud.attributes.new("hi_phy_constraint_direction", 'FLOAT_VECTOR', 'POINT')
    direction_attr.data.foreach_set('vector', constraint_direction);

    pointcloud.update()
    return True
