import os
import numpy as np
import cv2
import shutil
from scipy.spatial.distance import euclidean as d_euclidean
def read_tracking_output(filepath):
    '''
    0/1_tracking_output.data 数据读取
    '''
    boxes = []
    feats = []
    gift_data = []
    
    if not os.path.isfile(filepath):
        return boxes, feats, gift_data

    with open(filepath, 'r', encoding='utf-8') as file:
        lines = file.readlines()
        
        # 处理常规的boxes和feats数据
        for line in lines:
            line = line.strip()
            if not line:
                continue
            if line.find("gift identify result:") >= 0:
                break
                
            if line.endswith(','):
                line = line[:-1]
                
            try:
                data = np.array([float(x) for x in line.split(",")])
                if data.size == 9:
                    boxes.append(data)
                if data.size == 256:
                    feats.append(data)
            except ValueError:
                continue
                
        # 提取gift identify result和select back tracking feats index之间的数据
        try:
            start_idx = -1
            end_idx = -1
            for i, line in enumerate(lines):
                if "gift identify result:" in line:
                    start_idx = i + 1
                elif "select back tracking feats index:" in line:
                    end_idx = i
                    break
                    
            if start_idx != -1 and end_idx != -1:
                content = []
                for i in range(start_idx, end_idx):
                    line = lines[i].strip()
                    if line:
                        content.append(line)
                # 将所有内容合并成一行字符串
                if content:
                    gift_data.append(' '.join(content))
        except Exception as e:
            print(f"Error extracting gift data: {e}")

    if len(feats) != len(boxes) or len(boxes) == 0:
        return [], [], gift_data

    return [np.array(boxes)], [np.array(feats)], gift_data


def extract_data_realtime(datapath):
    boxes, feats, gift_data = read_tracking_output(datapath)
    
    if not boxes or not feats:
        return np.array([]), {}, []
        
    trackerboxes = boxes[0]  # 因为read_tracking_output返回的是list中的numpy数组
    trackerfeats = feats[0]

    frmIDs = np.sort(np.unique(trackerboxes[:, 7].astype(int)))
    for fid in frmIDs:
        idx = np.where(trackerboxes[:, 7] == fid)[0]
        box = trackerboxes[idx, :]
        feat = trackerfeats[idx, :]

        for i in range(len(box)):
            f, b = int(box[i, 7]), int(box[i, 8])
            tracker_feats.update({f"{f}_{b}": feat[i, :]})
    return trackerboxes, tracker_feats

def crop_box(imgpath, xyxy):
    img = cv2.imread(imgpath)
    box = img[int(xyxy[1]):int(xyxy[3]), int(xyxy[0]):int(xyxy[2]), :]
    return box

def copy_subimgs(video_path, file_name):

    for event in os.listdir(video_path):
        try:
            event_path = os.path.join(video_path, event)
            if len(os.listdir(event_path)) > 0:
                ori_path = os.path.join(event_path, file_name)
                sub_path = os.path.join(video_path+'_'+file_name, event, file_name)

                shutil.copytree(ori_path, sub_path)
                # shutil.rmtree(ori_path)
                print(f'copy {ori_path} to {sub_path}')
        except Exception as e:
            with open('error_copy_subimgs.txt', 'a') as f:
                f.write(f"{event} {e}\n")



def get_img_file(event_path):
    imgfile_list = []
    for img_file in os.listdir(event_path):
        img_path = os.path.join(event_path, img_file)
        if os.path.isdir(img_path):
            imgfile_list.append(img_file)
    return imgfile_list

def compute_box_dist(boxes, dist=50):
    # print('boxes', boxes)
    previous_centers = ()
    new_boxes = []
    for i, box in enumerate(boxes):
        if box[0] == box[1] == box[2] == box[3]:
            continue
        x1, y1, x2, y2 = box[:4]
        center = ((x1 + x2) / 2, (y1 + y2) / 2)

        if i == 0:
            # 如果前一帧没有中心点信息，则直接添加
            new_boxes.append(box)
            previous_centers = center
        else:
            distances = np.linalg.norm(np.array(center) - np.array(previous_centers))

            if distances > dist:
                # 如果所有距离都大于等于100，则添加当前box
                new_boxes.append(box)
                previous_centers = center

    return new_boxes


class Trajectory():
    # def __init__(self, boxes_list):
    #     self.boxes = boxes_list
    def minimum_desription_length(self, start_idx, curr_idx, trajectory, w_angular=1, w_perpendicular=1, par=True,
                                  directional=True):
        """
            Calculate the minimum description length.
        """
        LH = LDH = 0
        for i in range(start_idx, curr_idx - 1):
            ed = d_euclidean(trajectory[i], trajectory[i + 1])
            # print("ed:", ed)
            LH += max(0, np.log2(ed, where=ed > 0))
            if par:
                for j in range(start_idx, i - 1):
                    # print()
                    # print(np.array([trajectory[start_idx], trajectory[i]]))
                    # print(np.array([trajectory[j], trajectory[j+1]]))
                    LDH += w_perpendicular * self.d_perpendicular(np.array([trajectory[start_idx], trajectory[i]]),
                                                             np.array([trajectory[j], trajectory[j + 1]]))
                    LDH += w_angular * self.d_angular(np.array([trajectory[start_idx], trajectory[i]]),
                                                 np.array([trajectory[j], trajectory[j + 1]]), directional=directional)
                    # print("LDH:", LDH)
        if par:
            return LDH + LH
        return LH
    def partition(self, trajectory, directional=True, progress_bar=False, edis=30, w_perpendicular=1, w_angular=1):
        """
            Partition a trajectory into segments.
        """
        # Ensure that the trajectory is a numpy array of shape (n, 2)
        if not isinstance(trajectory, np.ndarray):
            raise TypeError("Trajectory must be a numpy array")
        elif trajectory.shape[1] != 2:
            raise ValueError("Trajectory must be a numpy array of shape (n, 2)")

        # Initialize the characteristic points, add the first point as a characteristic point
        cp_indices = []
        cp_indices.append(0)

        traj_len = trajectory.shape[0]
        start_idx = 0

        length = 1
        while start_idx + length < traj_len:
            if progress_bar:
                print(f'\r{round(((start_idx + length) / traj_len) * 100, 2)}%', end='')
            # print(f'Current Index: {start_idx + length}, Trajectory Length: {traj_len}')
            curr_idx = start_idx + length
            # print(start_idx, curr_idx)
            # print(f"Current Index: {curr_idx}, Current point: {trajectory[curr_idx]}")
            cost_par = self.minimum_desription_length(start_idx, curr_idx, trajectory, w_angular=w_angular,
                                                 w_perpendicular=w_perpendicular, directional=directional)
            cost_nopar = self.minimum_desription_length(start_idx, curr_idx, trajectory, par=False, directional=directional)
            # cost_par += 0 if 1 - cos == 0 else w_feats / (1 - cos)
            # print(f'Cost with partition: {cost_par}, Cost without partition: {cost_nopar}')
            if cost_par > cost_nopar and d_euclidean(trajectory[start_idx], trajectory[curr_idx]) > edis:
                # print('edp:', d_euclidean(trajectory[start_idx], trajectory[curr_idx]))
                # print(f"Added characteristic point: {trajectory[curr_idx-1]} with index {curr_idx-1}")
                cp_indices.append(curr_idx - 1)
                start_idx = curr_idx - 1
                length = 1
            else:
                length += 1

        # Add last point to characteristic points
        cp_indices.append(len(trajectory) - 1)
        # print(cp_indices)

        return np.array([trajectory[i] for i in cp_indices]), cp_indices

    def d_angular(self, l1, l2, directional=True):
        """
            Calculate the angular distance between two lines.
        """

        # Find the shorter line and assign that as l_shorter
        l_shorter = l_longer = None
        l1_len, l2_len = d_euclidean(l1[0], l1[-1]), d_euclidean(l2[0], l2[-1])
        if l1_len < l2_len:
            l_shorter = l1
            l_longer = l2
        else:
            l_shorter = l2
            l_longer = l1

        # Get the minimum intersecting angle between both lines
        shorter_slope = (l_shorter[-1, 1] - l_shorter[0, 1]) / (l_shorter[-1, 0] - l_shorter[0, 0]) if l_shorter[
                                                                                                           -1, 0] - \
                                                                                                       l_shorter[
                                                                                                           0, 0] != 0 else np.inf
        longer_slope = (l_longer[-1, 1] - l_longer[0, 1]) / (l_longer[-1, 0] - l_longer[0, 0]) if l_longer[-1, 0] - \
                                                                                                  l_longer[
                                                                                                      0, 0] != 0 else np.inf

        # The case of a vertical line
        theta = None
        if np.isinf(shorter_slope):
            # Get the angle of the longer line with the x-axis and subtract it from 90 degrees
            tan_theta0 = longer_slope
            tan_theta1 = tan_theta0 * -1
            theta0 = np.abs(np.arctan(tan_theta0))
            theta1 = np.abs(np.arctan(tan_theta1))
            theta = min(theta0, theta1)
        elif np.isinf(longer_slope):
            # Get the angle of the shorter line with the x-axis and subtract it from 90 degrees
            tan_theta0 = shorter_slope
            tan_theta1 = tan_theta0 * -1
            theta0 = np.abs(np.arctan(tan_theta0))
            theta1 = np.abs(np.arctan(tan_theta1))
            theta = min(theta0, theta1)
        else:
            tan_theta0 = (shorter_slope - longer_slope) / (1 + shorter_slope * longer_slope)
            tan_theta1 = tan_theta0 * -1

            theta0 = np.abs(np.arctan(tan_theta0))
            theta1 = np.abs(np.arctan(tan_theta1))

            theta = min(theta0, theta1)

        if directional:
            return np.sin(theta) * d_euclidean(l_longer[0], l_longer[-1])

        if 0 <= theta < (90 * np.pi / 180):
            return np.sin(theta) * d_euclidean(l_longer[0], l_longer[-1])
        elif (90 * np.pi / 180) <= theta <= np.pi:
            return np.sin(theta)
        else:
            raise ValueError("Theta is not in the range of 0 to 180 degrees.")

    def get_distance_optimized_bboxes(self, boxes):
        points = []
        for box in boxes:
            points.append([int(box[2]), int(box[3])])  # points 取x2,y2的坐标
        points = np.array(points)
        print("&&&&&&&&&&&&&&&&&&", points.shape)
        partitions, indices = self.partition(points, progress_bar=False, w_perpendicular=100, w_angular=10)
        bboxes_opt = []
        for indice in indices:
            bboxes_opt.append(boxes[indice])
        return bboxes_opt

    def d_perpendicular(self, l1, l2):
        """
            Calculate the perpendicular distance between two lines.
        """
        # Find the shorter line and assign that as l_shorter
        l_shorter = l_longer = None
        l1_len, l2_len = d_euclidean(l1[0], l1[-1]), d_euclidean(l2[0], l2[-1])
        if l1_len < l2_len:
            l_shorter = l1
            l_longer = l2
        else:
            l_shorter = l2
            l_longer = l1

        ps = self.get_point_projection_on_line(l_shorter[0], l_longer)
        pe = self.get_point_projection_on_line(l_shorter[-1], l_longer)

        lehmer_1 = d_euclidean(l_shorter[0], ps)
        lehmer_2 = d_euclidean(l_shorter[-1], pe)

        if lehmer_1 == 0 and lehmer_2 == 0:
            return 0
        return (lehmer_1 ** 2 + lehmer_2 ** 2) / (lehmer_1 + lehmer_2)  # , ps, pe, l_shorter[0], l_shorter[-1]

    def get_point_projection_on_line(self, point, line):
        """
            Get the projection of a point on a line.
        """

        # Get the slope of the line using the start and end points
        line_slope = (line[-1, 1] - line[0, 1]) / (line[-1, 0] - line[0, 0]) if line[-1, 0] != line[0, 0] else np.inf

        # In case the slope is infinite, we can directly get the projection
        if np.isinf(line_slope):
            return np.array([line[0, 0], point[1]])

        # Convert the slope to a rotation matrix
        R = self.slope_to_rotation_matrix(line_slope)

        # Rotate the line and point
        rot_line = np.matmul(line, R.T)
        rot_point = np.matmul(point, R.T)

        # Get the projection
        proj = np.array([rot_point[0], rot_line[0, 1]])

        # Undo the rotation for the projection
        R_inverse = np.linalg.inv(R)
        proj = np.matmul(proj, R_inverse.T)

        return proj

    def slope_to_rotation_matrix(self, slope):
        """
            Convert slope to rotation matrix.
        """
        return np.array([[1, slope], [-slope, 1]])

# video_path = "20250402_gift_test"
# video_path = "gift_test"
video_path = "/testDataAndLogs/测试视频数据以及日志/各模块测试记录/赠品测试/20250512赠品测试数据"
track_list = ['0_tracking_output.data', '1_tracking_output.data']
del_staticBox = True    #计算当前帧box与前一帧box中心点距离，距离小于50删除当前帧
filter_track = False     #使用轨迹过滤算法筛选box
save_sub = '../data/negtive_dm50'

traclus = Trajectory()


for event in os.listdir(video_path):
    # try:
    event_path = os.path.join(video_path, event)
    print('event_path', event_path)
    if len(os.listdir(event_path)) > 0:
        imgfile_list = get_img_file(event_path)
        # print('imgfile_list', imgfile_list)
        for track_data in track_list:
            track_path = os.path.join(event_path, track_data)
            boxes, feat, gift_data = extract_data_realtime(track_path)
            camera_id = track_data.split('_')[0]
            imgfile = [x for x in imgfile_list if x.split('_')[0] == camera_id][0]
            
            # 打印gift数据
            if gift_data:
                print(f"\nGift data for {event}/{track_data}:")
                print(gift_data[0])  # 现在gift_data只包含一个元素，即合并后的字符串
                    
            if len(boxes) > 0:
                if del_staticBox:   ##根据距离删除box
                    boxes_ = compute_box_dist(boxes)
                elif filter_track:
                    boxes_ = traclus.get_distance_optimized_bboxes(boxes)
                else:
                    boxes_ = boxes
                # print('************box', boxes_)
                for i, box in enumerate(boxes_):

                    if box[0] == box[1] == box[2] == box[3]:
                        break
                    xyxy = box[:4]
                    # print('0xyxy', xyxy)
                    frame_id = int(box[7])
                    imgpath = os.path.join(event_path, imgfile, f"{imgfile}_{frame_id}.jpg")
                    # print('imgpath', imgpath)
                    subimg = crop_box(imgpath, xyxy)
                    if len(subimg) > 0:
                        save_path = os.path.join(event_path, save_sub)
                        if not os.path.exists(save_path):
                            os.makedirs(save_path)
                        save_subimg = os.path.join(save_path, f"{imgfile}_{frame_id}.jpg")
                        cv2.imwrite(save_subimg, subimg)
                        print(f'save imgs:{save_subimg}')
                    break
                break
    # except Exception as e:
    #     with open('error_tracluster_event.txt', 'a') as f:
    #         f.write(f"{event} {e}\n")



# # '''将subimgs文件夹单独复制出来'''
# file_name = 'sub_imgs_tracluster'
# copy_subimgs(video_path, file_name)