import sys
import numpy as np
from sklearn.cluster import KMeans
from cadquery.selectors import NearestToPointSelector, DirectionSelector
root_dir = r'/home/thebears/Seafile/Designs/Projects/kickdrawers/cadfree/code'
sys.path.insert(0,root_dir)

import items as it 
import cadquery as cq
import util as u
from cadquery.selectors import NearestToPointSelector


def make_fingers(o1,o2, n_tabs = 5, slots_1 = True, slots_2 = True, move_to_slice = (0,0,0), flip_slot_size = False, slot_1_offset = 0, slot_2_offset = 0, t = 3.5):
    o_int = u.intersect(o1, o2)
    verts = u.vertices_to_array(o_int.vertices().vals())
    km = np.asarray(KMeans(2).fit_predict(verts))

    o1_com = cq.Shape.centerOfMass(o1.objects[0])
    o2_com = cq.Shape.centerOfMass(o2.objects[0])
    
    
    f_sel_1 = NearestToPointSelector((o1_com.x, o1_com.y, o1_com.z)) 
    f_vals_1 = u.vertices_to_array(o_int.faces(f_sel_1).vertices() .vals())
    vec_norm_1 = get_plane_norm(f_vals_1, o1)
    dir_1_sel = DirectionSelector(vec_norm_1)              

    
    f_sel_2 = NearestToPointSelector((o2_com.x, o2_com.y, o2_com.z))
    f_vals_2 = u.vertices_to_array(o_int.faces(f_sel_2).vertices().vals())
    vec_norm_2 = get_plane_norm(f_vals_2, o2)
    dir_2_sel = DirectionSelector(vec_norm_2)   

    g_start = np.average(verts[km==0,:], axis=0)
    g_end = np.average(verts[km==1,:],axis=0)
    if g_start[-1] > g_end[-1]:
        g_start = np.average(verts[km==1,:], axis=0)
        g_end = np.average(verts[km==0,:],axis=0)

    g_dir = g_end - g_start

    g_step = tuple(g_dir / (n_tabs ))
    ndiff = np.linalg.norm(g_step)
    

    
    ccurr = o_int.translate(move_to_slice)
    neg_move_to_slice = tuple([-x for x in move_to_slice])
    tabs = u.ListMod()
    for i in range(n_tabs):
        p_sel = NearestToPointSelector(g_end)
        verts = u.vertices_to_array(ccurr.faces(p_sel).vertices().vals())
        cc = ccurr.faces(p_sel).workplane(-ndiff).split(keepTop=True, keepBottom=True)
        
        tabs+= cc.newObject(cc.objects[0].translate(neg_move_to_slice))
        ccurr = cc.newObject(cc.objects[1])
    
    # tabs += ccurr
    o1 = o1.cut(o_int) 
    o2 = o2.cut(o_int)
    if flip_slot_size:
        slot_size = (3,t)
        nut_size = (5.5,t)
    else:
        slot_size = (t,3)
        nut_size = (t,5.5)

    for idx, tab in enumerate(tabs):
        if idx % 2 == 0:

            o1 = o1.union(tab)
            if slots_1:
                o2 = o2.cut(tab.faces(dir_2_sel).workplane(centerOption="CenterOfMass").center(slot_1_offset,0).rect(*slot_size).extrude(10).cut(tab))
                wplane = tab.faces(dir_2_sel).workplane(centerOption="CenterOfMass").center(slot_1_offset,0)
                tab_w_hole = wplane.circle(2).extrude(t*10)
                tab_w_hole = tab_w_hole.cut(tab).translate(-t*3*vec_norm_2)
                o2 = o2.cut(tab.faces(dir_2_sel).workplane(5,centerOption="CenterOfMass").center(slot_1_offset,0).rect(*nut_size).extrude(2.5))
                o1 = o1.cut(tab_w_hole)            
        else:        
            
            o2 = o2.union(tab)
            if slots_2:
                o1 = o1.cut(tab.faces(dir_1_sel).workplane(centerOption="CenterOfMass").center(slot_2_offset,0).rect(*slot_size).extrude(10).cut(tab))
                wplane = tab.faces(dir_1_sel).workplane(centerOption="CenterOfMass").center(slot_2_offset,0)
                tab_w_hole = wplane.circle(2).extrude(t*10)
                
                tab_w_hole = tab_w_hole.cut(tab).translate(-t*3*vec_norm_1)
                o1 = o1.cut(tab.faces(dir_1_sel).workplane(5,centerOption="CenterOfMass").center(slot_2_offset,0).rect(*nut_size).extrude(2.5))
                o2 = o2.cut(tab_w_hole)
            

            

    return (o1,o2)

def get_plane_norm(f_vals_1, oshape = None):
    vec1 = cq.Vector(*(f_vals_1[1] - f_vals_1[2]))
    vec2 = cq.Vector(*(f_vals_1[3] - f_vals_1[2]))
    vec_norm = vec1.cross(vec2).normalized()

    
    if oshape is not None:
        vec_dir = cq.Vector(*(f_vals_1[1])) - cq.Shape.centerOfMass(oshape.objects[0])
        
        if vec_norm.dot(vec_dir) > 0:
            vec_norm = -vec_norm
    return vec_norm