import torch
import numpy as np
import cv2
import matplotlib.pyplot as plt
from IPython.display import clear_output
import os
from PIL import Image
import pandas as pd
import time
import natsort
from tqdm import tqdm

from multicam_utils import give_color
from cython_bbox import bbox_overlaps as bbox_ious

import cv2
import time
import torch
import torch.nn.functional as F
import argparse
import numpy as np
import matplotlib.pyplot as plt
from torchvision import transforms

import copy
import scipy
import lap
from scipy.spatial.distance import cdist
import warnings

DISTANCE = 100

# %matplotlib inline
warnings.filterwarnings("ignore")

# load obj_img and calculate average feat

# average feat based matching

# new id is in then get average

tracking_result_path = '/data/cvprw/AIC23/tracking/SUBMISSION/S021'

channel_list = []
temp = [file for file in os.listdir(tracking_result_path)]
for channel in temp:
    d = os.path.join(tracking_result_path, channel)
    if os.path.isdir(d):
#         print(channel)
        channel_list.append(channel)

channel_list=natsort.natsorted(channel_list)

del channel_list[0]
channel_list


# Open two video capture objects
cap1 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[0], 'video.mp4'))
cap2 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[1], 'video.mp4'))
cap3 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[2], 'video.mp4'))
cap4 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[3], 'video.mp4'))
cap5 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[4], 'video.mp4'))
cap6 = cv2.VideoCapture(os.path.join(tracking_result_path, channel_list[5], 'video.mp4'))

_, frame1 = cap1.read()
_, frame2 = cap2.read()
_, frame3 = cap3.read()
_, frame4 = cap4.read()
_, frame5 = cap5.read()
_, frame6 = cap6.read()
                        
with open(os.path.join(tracking_result_path, channel_list[0], 'label.txt')) as label1:
    gt1 = label1.readlines()
with open(os.path.join(tracking_result_path, channel_list[1], 'label.txt')) as label2:
    gt2 = label2.readlines()
with open(os.path.join(tracking_result_path, channel_list[2], 'label.txt')) as label3:
    gt3 = label3.readlines()
with open(os.path.join(tracking_result_path, channel_list[3], 'label.txt')) as label4:
    gt4 = label4.readlines()
with open(os.path.join(tracking_result_path, channel_list[4], 'label.txt')) as label5:
    gt5 = label5.readlines()
with open(os.path.join(tracking_result_path, channel_list[5], 'label.txt')) as label6:
    gt6 = label6.readlines()


width = cap1.get(cv2.CAP_PROP_FRAME_WIDTH)  # float
height = cap1.get(cv2.CAP_PROP_FRAME_HEIGHT)  # float
fps = cap1.get(cv2.CAP_PROP_FPS)
vid_length = int(cap1.get(cv2.CAP_PROP_FRAME_COUNT))

def draw_bbox_tracking(img, results, thres):
    for obj in results:
        if obj[4]>thres:
            x = int(obj[0])
            y = int(obj[1])
            x2 = int(obj[2])
            y2 = int(obj[3])
            
            tracking_id = obj[5]
            
            color = give_color(tracking_id)
            
            tk = 3
            
            content = str(tracking_id)
            
            font =  cv2.FONT_HERSHEY_PLAIN
            img = cv2.rectangle(img, (x,y), (x2,y2), color, tk) # bbox
            img = cv2.putText(img, content, (x, y-2), font, tk, color, tk, cv2.LINE_AA) # label
    return img


def extract_tracking_result(gt, frame_count, W, H):
    track_bbox=[]
    for row in gt:
        if int(row.split(',')[0]) == frame_count:
            track_id=int(row.split(',')[1])
            
            x = int(max(0, int(float(row.split(',')[2]))))
            y = int(max(0, int(float(row.split(',')[3]))))
            if int(float(row.split(',')[4]))+x >W:
                obj_w=W-x-1
            else:
                obj_w=int(float(row.split(',')[4]))
                
            if int(float(row.split(',')[5]))+y >H:
                obj_h=H-y-1
            else:
                obj_h=int(float(row.split(',')[5]))
            
            track_bbox.append([x,y,x+obj_w,y+obj_h,float(row.split(',')[6]),track_id])
            
        elif int(row.split(',')[0]) > frame_count:
            break
            
    return track_bbox

    
def make_square(img):
    """
    Given an image as a cv2 object, add padding to make it square.
    """
    # Get the image dimensions
    height, width = img.shape[:2]

    # Determine the size of the square to make
    square_size = max(width, height)

    # Create a new image of the appropriate size, with a black background
    new_img = np.zeros((square_size, square_size, 3), dtype=np.uint8)

    # Determine where to paste the original image in the new image
    x_offset = (square_size - width) // 2
    y_offset = (square_size - height) // 2

    # Paste the original image in the center of the new image
    new_img[y_offset:y_offset+height, x_offset:x_offset+width] = img

    return new_img

def extract_global_position2(frame_bbox, H):
    # format (x,y,raw_track_id)
    c_x = (frame_bbox[0]+frame_bbox[2])/2
    bottom_y = frame_bbox[1]+(frame_bbox[3]-frame_bbox[1])*0.95

    point1 = np.array([[c_x, bottom_y, 1]])
    point2 = np.dot(H, point1.T)
    point2 /= point2[2]
        
    return int(point2[0]),int(point2[1])

def add_projected_point2(projected_point_group, obj_list, channel_idx, global_obj_mapping):
    # projected_point_group [[[],[]].[[]],...]
    # Fromat : (<channel_track_id> <project_x> <project_y> <reid_feat> <state_bit>), <channel_idx>)
    
    distance = DISTANCE # 두개 그룹 이상 매칭 되면 feat으로 한 그룹에 묶어줌
    
    if len(projected_point_group)==0:
        for row in obj_list:
            row.append(channel_idx)
            projected_point_group.append([row])
    else:
        for row in obj_list:
            match_idx = []
            for idx,group in enumerate(projected_point_group): # 클러스터링 된 수만큼 그룹수 존재 (idx)
                _, avg_x, avg_y = np.mean(np.array(group)[:,:3], axis=0)
                if (row[1] - avg_x)**2 + (row[2] - avg_y)**2 < distance**2: # distanec under 100pixel
                    match_idx.append(idx)

            if len(match_idx)==1:
                row.append(channel_idx)
                projected_point_group[match_idx[0]].append(row)
            elif len(match_idx)>1:
                # print("두개이상 매칭")
                min_distance = 0.5
                match_id = None
                for group_id in match_idx:
                    for obj in projected_point_group[group_id]:
                        if embedding_distance(obj[3],row[3])<min_distance:
                            match_id = group_id
                            
                if match_id is not None:
                    row.append(channel_idx)
                    projected_point_group[match_id].append(row)
                else:
                    print("두개 이상 매칭 되는데 유사한건 없음")
                    row.append(channel_idx)
                    projected_point_group.append([row])
                
            else:
                row.append(channel_idx)
                projected_point_group.append([row])

    return projected_point_group

def make_occlusion_matrix(dets):
    occluded_val_list = []
    if len(dets)>0:
        ious = bbox_ious(
            np.ascontiguousarray(dets, dtype=np.float64),
            np.ascontiguousarray(dets, dtype=np.float64)
        )

        for iou_row in ious:
            if sum(iou_row)>1.05:
                occluded_val_list.append(True)
            else:
                occluded_val_list.append(False)
                
    return occluded_val_list

def run_keypoint(frame_bbox, frame_img, keypoint_model, strong=True):
    thres = 0.5
    padding_size = 10
    
    # padding 40px and make square based on height
    padding_height = frame_bbox[3]-frame_bbox[1]+padding_size
    center_x = (frame_bbox[0]+frame_bbox[2])/2

    patch_x1 = int(max(0, frame_bbox[0]-padding_size))
    patch_y1 = int(max(0, frame_bbox[1]-padding_size))
    patch_x2 = int(min(W - 1, frame_bbox[2]+padding_size))
    patch_y2 = int(min(H - 1, frame_bbox[3]+padding_size))

    patch = frame_img[patch_y1:patch_y2, patch_x1:patch_x2, :]
    patch_squared = make_square(patch)

#                 resized_patch = cv2.resize(patch, (256,256))
    square_size = 256
    resized_patch = cv2.resize(cv2.cvtColor(patch_squared, cv2.COLOR_BGR2RGB), (square_size,square_size))
    
    # %matplotlib inline
    # plt.imshow(resized_patch)
    # plt.show()
    
    resized_patch = torch.tensor(np.array([transforms.ToTensor()(resized_patch).numpy()]))

    resized_patch = resized_patch.to("cuda:0")  #convert image data to device
    resized_patch = resized_patch.float() #convert image to float precision (cpu)

    with torch.no_grad():  #get predictions
        output_data, _ = keypoint_model(resized_patch)

    output_data = non_max_suppression_kpt(output_data,   #Apply non max suppression
                0.25,   # Conf. Threshold.
                0.65, # IoU Threshold.
                nc=keypoint_model.yaml['nc'], # Number of classes.
                nkpt=keypoint_model.yaml['nkpt'], # Number of keypoints.
                kpt_label=True)
    output = output_to_keypoint(output_data)
    
    total_crop = frame_img[frame_bbox[1]:frame_bbox[3], frame_bbox[0]:frame_bbox[2], :]

    obj_width = frame_bbox[2] - frame_bbox[0]
    obj_height = frame_bbox[3] - frame_bbox[1]
    # 전체 감지 : 2, 감지안됨 : 0
    state_bit = 0
    
    max_area = 10
    for o in output:
        obj_area = o[4]*o[5]
        if (obj_area>max_area) and (o[54]>thres or o[57]>thres) and (o[18]>thres or o[21]>thres or o[24]>thres or o[27]>thres):
            max_area = obj_area
            state_bit = 2  
        elif (obj_area>max_area) and (o[54]>thres or o[57]>thres):
            max_area = obj_area
            state_bit = 2  
    
    
    # 전체 인데 반만 감지되거나, 아예 감지되지 않는 경우도 있음.
    #감지는 안되지만 가로세로 비로 추정
    if not strong and obj_height>obj_width*2.5:
        state_bit = 2

    return state_bit, total_crop

def embedding_distance(tracks, detections, metric='cosine'):
    """
    :param tracks: list[STrack]
    :param detections: list[BaseTrack]
    :param metric:
    :return: cost_matrix np.ndarray
    """

    cost_matrix = np.zeros((len(tracks), len(detections)), dtype=np.float64)
    if cost_matrix.size == 0:
        return cost_matrix

    det_features = np.asarray([tracks], dtype=np.float64)
    track_features = np.asarray([detections], dtype=np.float64)

    cost_matrix = np.maximum(0.0, cdist(track_features, det_features, metric))  # / 2.0  # Nomalized features
    return cost_matrix

def postprocess(features):
    # Normalize feature to compute cosine distance
    features = F.normalize(features)
    features = features.cpu().data.numpy()
    return features

def extract_reid_feature(img, model):
    
    h,w,c = img.shape
    
    img = cv2.resize(img, tuple((100,300)), interpolation=cv2.INTER_LINEAR)
    
    patches = []
    
    patch_img = torch.as_tensor(img.astype("float32").transpose(2, 0, 1))
    patch_img = patch_img.to(device="cuda:0").half()
    patches.append(patch_img)
    
    if len(patches):
        patches = torch.stack(patches, dim=0)

    features = np.zeros((0, 2048))
    
    # Run model
    patches_ = torch.clone(patches)
    pred = model(patches)
    pred[torch.isinf(pred)] = 1.0

    feat = postprocess(pred)

    return feat[0]
    
def check_occlusion(local_obj_mapping):
    
    occlusion_xy = [] 
    
    distance = DISTANCE
    
    # ***한채널에 대해서 분석해서 글로벌 좌표 뽑음
    for channel_obj in local_obj_mapping:
        for obj1 in channel_obj:
            for obj2 in channel_obj:
                if (obj1 != obj2) and (obj1[-1] == 2 and obj2[-1] == 2) and ((obj1[1]-obj2[1])**2+(obj1[2]-obj2[2])**2) < distance**2:
                    occlusion_xy.append((int((obj1[1]+obj2[1])/2), int((obj1[2]+obj2[2])/2)))
        
    # local -> [[],[],[],[],[]]
    for idx, channel_obj in enumerate(local_obj_mapping):
        for jdx, obj in enumerate(channel_obj):
            if obj[-1] == 2:
                for item in occlusion_xy:
                    if (item[0]-obj[1])**2 + (item[1]-obj[2])**2 < (distance/2+10)**2:
                        local_obj_mapping[idx][jdx][-1]=3
                    
    return local_obj_mapping

def update_feature(origin_feat_list, new_feat_list):
    origin_feat_list = list(origin_feat_list)
    new_feat_avg = np.mean(new_feat_list, axis=0)
    
    update_bit = False
    for idx, feat in enumerate(origin_feat_list):
        if embedding_distance(feat,new_feat_avg)<0.05:
            update_bit = True
            origin_feat_list[idx]= origin_feat_list[idx]*0.999 + new_feat_avg*0.001
            
    if not update_bit:
        origin_feat_list.append(new_feat_avg)
        
    new_feat_list = np.array(origin_feat_list)

    return new_feat_list



pts1 = np.array([(1753, 515), (1348, 411),(869, 289),(476, 231),(624, 227),(1093, 166)])
pts2 = np.array([(1065, 197), (1065, 342),(1065, 562),(1108, 753),(1065, 709),(858, 562)])
H_c124, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC)

pts1 = np.array([(176,447),(692,251),(1074,156),(400,147),(635,80),(929,161)])
pts2 = np.array([(1065,562),(1065,342),(1108,153),(858,342),(861,197),(1065,197)])
H_c125, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC)

# pts1 = np.array([(566, 488), (968, 548), (718,487), (1495,674), (1210, 449), (1393, 280)])
# pts2 = np.array([(817, 153), (861, 342), (861,197), (861,562), (1065, 342), (1598, 197)])
pts1 = np.array([(566, 488), (968, 548), (718,487), (1495,674), (1210, 449),(1413,477),(1305,320),(175,898)])
pts2 = np.array([(817, 153), (861, 342), (861,197), (861,562), (1065, 342),(1040,452),(1400,200),(520,350)])
H_c126, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC,5.0)

pts1 = np.array([(508,388),(220,605),(760,471),(1007,410),(1151,410),(323,233),(408,233)])
pts2 = np.array([(1065,562),(858,342),(858,562),(858,709),(817,753),(1598,709),(1554,753)])
H_c127, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC)

pts1 = np.array([(117, 858), (66, 588), (198, 488),(1219, 251),(1361, 265)])
pts2 = np.array([(858, 562), (858, 709),(817, 753),(369, 753),(325, 709)])
H_c128, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC)

pts1 = np.array([(30,756),(601,697),(1284,623),(1638,584),(1033,505),(926,350),(1006,355), (1854,604)])
pts2 = np.array([(1065,709),(1065,562),(1065,342),(1065,197),(858,342),(325,197),(367,153), (1108,153)])
H_c129, _ = cv2.findHomography(pts1, pts2, cv2.RANSAC, 5.0)


H_list = [H_c124,H_c125,H_c126,H_c127,H_c128,H_c129]


from models.experimental import attempt_load
from utils.general import non_max_suppression_kpt,strip_optimizer,xyxy2xywh
from utils.plots import output_to_keypoint, plot_skeleton_kpts,colors,plot_one_box_kpt

poseweights = "yolov7-w6-pose.pt"
device = "cuda:0"

keypoint_model = attempt_load(poseweights, map_location=device)  #Load model
_ = keypoint_model.eval()

from fast_reid.fastreid.config import get_cfg
from fast_reid.fastreid.modeling.meta_arch import build_model
from fast_reid.fastreid.utils.checkpoint import Checkpointer
from fast_reid.fastreid.engine import DefaultTrainer, default_argument_parser, default_setup, launch

def setup_cfg(config_file, opts):
    # load config from file and command-line arguments
    cfg = get_cfg()
    cfg.merge_from_file(config_file)
    cfg.merge_from_list(opts)
    cfg.MODEL.BACKBONE.PRETRAIN = False

    cfg.freeze()

    return cfg

config_file = '/data/cvprw/AIC23/BoT-SORT/logs/AIC23/sbs_S101/config.yaml'
weights_path = '/data/cvprw/AIC23/BoT-SORT/logs/AIC23/sbs_S101/model_0099.pth'

cfg = setup_cfg(config_file, ['MODEL.WEIGHTS', weights_path])

reid_model = build_model(cfg)
reid_model.eval()

Checkpointer(reid_model).load(weights_path)

reid_model = reid_model.eval().to(device='cuda:0').half()

start = 0

# channel_dic c025~c029
# Format : ({channel},{},...) | channel -> (<channel_track_id> : <global_track_id>)
channel_id_mapping = [{},{},{},{},{},{}]

# global_dic
# Format : ({id},{},...) | 
# id -> (<global_track_id> : <projected_x>, <projected_y>, [<feat_total>,...])
global_obj_mapping = {}

# loss for head only or occlusion
# channel loss c025~c029
# Format : channel_loss_mapping -> ([],[],...) | 
# channel -> (<frame_count> <channel_idx>  <channel_result>)
channel_loss_final_output = [[],[],[],[],[],[]]

# Save Final Result
# Format : channel_final_output -> ([channel],[],...)
# channel -> (〈channel_idx〉 〈global_track_id〉 〈frame_id〉 〈xmin〉 〈ymin〉 〈width〉 〈height〉 〈xworld〉 〈yworld〉)
#  ==> <channel_result>
channel_final_output = [[],[],[],[],[],[]]


map_img = cv2.imread(os.path.join(tracking_result_path,'map.png'))

def add_local2gloabl(global_obj_mapping, channel_id_mapping, local_obj_mapping, new_id):
    
    local_channel_loss_mapping = [[],[],[],[],[],[]]
    # projected_point_group [[[],[]].[[]],...]
    # Fromat : (<channel_track_id>, <projected_x>, <projected_y>, <reid_feat>, <state_bit>, <channel_idx>)
    projected_point_group = []
    
    # local_obj_mapping
    # Format : ([channel],[],...) |
    # id -> ( <channel_track_id> <project_x> <project_y> <reid_feat> <state_bit>)
    
    #1. Projection 기반 ID 생성
    for idx,channel_obj in enumerate(copy.deepcopy(local_obj_mapping)):
        temp = []
        for obj in channel_obj:
            if obj[-1]==2:
                temp.append(obj)
        projected_point_group = add_projected_point2(projected_point_group, temp, idx, global_obj_mapping)
        
    for group in projected_point_group:
        _, avg_x, avg_y = np.mean(np.array(group)[:,:3], axis=0)
        
        # 글로벌 좌표 추가, 여기에다가 feat도 추가해야함
        # (<global_track_id> : <projected_x>, <projected_y>, [<feat_total>,...])
        global_obj_mapping[new_id] = [int(avg_x), int(avg_y), np.array(group)[:,3]]
        
        # 각 채널 매핑
        for row in group:
            channel_id_mapping[row[-1]][row[0]] = new_id
        
        new_id+=1
    
    for idx,channel_obj in enumerate(copy.deepcopy(local_obj_mapping)):
        for obj in channel_obj:
            if obj[-1]!=2:
                local_channel_loss_mapping[idx].append(obj)
    
    return global_obj_mapping, channel_id_mapping, local_channel_loss_mapping, new_id



def match_local2gloabl(global_obj_mapping, channel_id_mapping, local_obj_mapping):
    # 틀리거나 feature 안맞으면 loss에 추가, 이때 type은 2로 그대로 가져가서 재 배치가 가능하게끔 함. -> reid 사용 하고 실패하면 add
    local_channel_loss_mapping = [[],[],[],[],[],[]]
    
    origin_channel_id_mapping = copy.deepcopy(channel_id_mapping)

    projected_point_group = []
    # #1. Projection 기반 먼저추가
    for idx,channel_result in enumerate(copy.deepcopy(local_obj_mapping)):
        temp = []
        for obj in channel_result:
            if obj[-1]==2:
                temp.append(obj)
        projected_point_group = add_projected_point2(projected_point_group, temp, idx, global_obj_mapping)
    
    
    # 그룹간 FEAT 출력
    # print("그룹간 FEAT 출력")
    # for group1 in projected_point_group:
    #     for group2 in projected_point_group:
    #         if group1 != group2:
    #             print(embedding_distance(np.mean(np.array(group1)[:,3], axis=0),np.mean(np.array(group2)[:,3], axis=0)))
            
            
    # None 없음, 2에 대해서만
    matched_id_list = []
    for group in projected_point_group:
        _, avg_x, avg_y = np.mean(np.array(group)[:,:3], axis=0)
        
        # 이전 글로벌 매핑과 매칭
        # 글로벌 좌표 추가, 여기에다가 feat도 추가해야함 상,하체 모두
        already_matched_global_id = np.array(matched_id_list)
        
        defined_global_id = []
        for key in global_obj_mapping.keys():
            if global_obj_mapping[key][0] is not None: # None 이 아닌 x,y좌표애 대해서만
                defined_global_id.append(key)
        defined_global_id = np.array(defined_global_id)
        
        target_global_id = np.setdiff1d(defined_global_id, already_matched_global_id)
        
        match_id = None
        min_distance = DISTANCE**2
        
        for tg_id in target_global_id:
            row = global_obj_mapping[tg_id]
            frame_distance = (avg_x - row[0])**2 + (avg_y - row[1])**2
            if frame_distance<min_distance:
                match_id = tg_id
                min_distance = frame_distance
        
        if match_id is not None:
            state = 1 # 일관성 문제
            for row in group: # 그룹안에 있는거는 모두 type2
                if row[0] in list(channel_id_mapping[row[-1]].keys()):
                    if channel_id_mapping[row[-1]][row[0]] != match_id:
                        state = 0 # 문제상황
                               
            if state == 1: # 매핑 성공 X,Y,Feat 업데이트
                matched_id_list.append(match_id)
                
                # 근사한것에만 -> 기존*0.9 + 새로운*0.1, 아니면 append
                origin_feat_list = global_obj_mapping[match_id][2]
                new_feat_list = update_feature(origin_feat_list, np.array(group)[:,3])
                # group 내 하나의 리스트 : <channel_track_id> <project_x> <project_y> <reid_feat> <state_bit> <channel_id>
                
                global_obj_mapping[match_id] = [int(avg_x), int(avg_y), new_feat_list]
                for row in group: # 각 채널상에 나타난 새로운 id 추가
                    channel_id_mapping[row[-1]][row[0]] = match_id
                
            else:
                # print("채널간 매핑 정보가 다름")
#                 # # print(channel_id_mapping)
#                 # for row in group: # 그룹안에 있는거는 모두 type2
#                 #     if row[0] in list(channel_id_mapping[row[-1]].keys()):
#                 #         if channel_id_mapping[row[-1]][row[0]] != match_id:
#                 #             # print("channel",row[-1],"channel_track_id",row[0])
#                 #             # print(match_id)
                            
                for row in group:
                    local_channel_loss_mapping[row[-1]].append(list(np.array(row)[:-1]))
                    channel_id_mapping[row[-1]].pop(row[0], None) # 채널 매핑 정보 지워줌
                    
        else: 
            # print("매치 아이디가 none 임")
            for row in group:
                local_channel_loss_mapping[row[-1]].append(list(np.array(row)[:-1]))    
    
    # print("그룹핑 결과")
    # for idx, group in enumerate(projected_point_group):
    #     print(idx)
    #     print(np.array(group))
    
    # 업데이트 안된 글로벌 좌표?? -> None
    for global_key in list(global_obj_mapping.keys()):
        if global_key not in matched_id_list:
            global_obj_mapping[global_key][0] = None
            global_obj_mapping[global_key][1] = None

    return global_obj_mapping, channel_id_mapping, local_channel_loss_mapping



def process_online(global_obj_mapping, channel_id_mapping, local_channel_loss_mapping, new_id):
    
    for idx,channel_obj in enumerate(local_channel_loss_mapping):
        for jdx,obj in enumerate(channel_obj):
            if obj[-1] == 2:
                defined_global_id = np.array(list(global_obj_mapping.keys())) # 모든 key
                
                target_global_id = defined_global_id
                
                # 자기 그룹내 re-id에 대해서, 글로벌 feature 들과 비교한 값중에 최솟값을 가지는 그룹이랑 글로벌 id 매칭
                # 단순 비교가 아닌 글로벌 feature와 현재 매핑되어 있는 feature간 거리 vs 글로벌 feature와 매핑할 feature간 거리
                min_distance = 0.08
                match_id = None
                
                temp = []
                
                for key in target_global_id:
                    # 해당 글로벌 id 에 해당하는 obj가 존재한다면 -> 그것과 글로벌 좌표간 매칭 보다 작아야 하고
                    row = global_obj_mapping[key]
                    if row[0] is not None: # 현재 프레임에서 업데이트되었으니, obj 존재
                        temp.append((obj[1] - row[0])**2 + (obj[2] - row[1])**2)
                        if (obj[1] - row[0])**2 + (obj[2] - row[1])**2 < 330**2:
                            # 새로운 OBJ 거리
                            for feat1 in row[2]:
                                now_dist = embedding_distance(np.array(feat1),np.array(obj[3]))
                                if now_dist<min_distance:
                                    min_distance = now_dist
                                    match_id = key

                    else : 
                        for feat1 in global_obj_mapping[key][2]:
                            now_dist = embedding_distance(np.array(feat1),np.array(obj[3]))
                            if now_dist+0.02<min_distance:
                                match_id = key
                                min_distance = now_dist

                if match_id is not None: # 성공
                    # origin_feat_list = global_obj_mapping[match_id][2]
                    # new_feat_list = update_feature(origin_feat_list, np.array(group)[:,3])
                    
                    if global_obj_mapping[match_id][0] is None:  
                        global_obj_mapping[match_id][0] = int(obj[1])
                        global_obj_mapping[match_id][1] = int(obj[2])
                        channel_id_mapping[idx][obj[0]] = match_id
                    else:
                        channel_id_mapping[idx][obj[0]] = match_id

                else: # 실패
                    print()
                    print("ID 증가")
                    print(new_id)
                    print(temp)
                    print()
                    # print("연관된 채널에서 '방금전 프레임'에서 정의된 글로벌 ID (제외대상)")
                    # print(matched_id_list)

                    # (<global_track_id> : <projected_x>, <projected_y>, [<feat_total>,...])
                    global_obj_mapping[new_id] = [int(obj[1]), int(obj[2]), np.array([obj[3]])]
                    channel_id_mapping[idx][obj[0]] = new_id

                    new_id+=1
                   
    # 채널 로스 local 로 돌리고 type2인건 다 처리 했으니 return 할필요 없지 않나?
    return global_obj_mapping, channel_id_mapping, new_id
    



def process_loss(channel_loss_final_output, channel_final_output, channel_id_mapping):
    
    new_channel_loss_final_output = [[],[],[],[],[],[]]
    
    for idx, channel_obj in enumerate(channel_loss_final_output):
        for obj in channel_obj: # obj -> (final_format , local_obj_format)
            channel_track_id = obj[1][0]
            if channel_track_id in channel_id_mapping[idx].keys():
                
                submit_format = obj[0]
                submit_format[1] = channel_id_mapping[idx][channel_track_id]
                
                channel_final_output[idx].append(submit_format)
                
            else:
                new_channel_loss_final_output[idx].append(obj)
    
    return new_channel_loss_final_output, channel_final_output
    
    
frame_count = 0
# end = 500
new_id = 100

gts=[gt1,gt2,gt3,gt4,gt5,gt6]

t = tqdm(total=vid_length)

while frame1 is not None:
    # read GT
    H, W, _ = np.shape(frame1)
    
    # format (x,y,x2,y2,det_score,track_id)
    track_bbox1 = extract_tracking_result(gt1, frame_count, W, H)
    track_bbox2 = extract_tracking_result(gt2, frame_count, W, H)
    track_bbox3 = extract_tracking_result(gt3, frame_count, W, H)
    track_bbox4 = extract_tracking_result(gt4, frame_count, W, H)
    track_bbox5 = extract_tracking_result(gt5, frame_count, W, H)
    track_bbox6 = extract_tracking_result(gt6, frame_count, W, H)
    
    track_result = [track_bbox1,track_bbox2,track_bbox3,track_bbox4,track_bbox5,track_bbox6]
    channel_frame = [frame1,frame2,frame3,frame4,frame5,frame6]
    
    # (1) 혼자인지 겹치는지 | 채널 하나에 대하여 각각 분석
    # 채널 ID는 0,1,2,3,4로 통일
    
    # Format : ([channel_id],[],...) |
    # channel_id ->  <channel_track_id> <project_x> <project_y> <reid_feat> <state_bit>
    local_obj_mapping = [[],[],[],[],[],[]]
    # local_channel_loss_mapping = [[],[],[],[],[],[]]
    
    
    for idx,track_bbox in enumerate(track_result):
        if len(track_bbox)>0:
            occlusion_check=make_occlusion_matrix(np.array(track_bbox)[:,:4])
            for jdx,v in enumerate(occlusion_check):
                frame_img = channel_frame[idx]
                H, W, _ = np.shape(frame_img)
                # format : x1,y1,x2,y2,score,track_id
                frame_bbox = track_bbox[jdx] 
                frame_img = np.array(channel_frame[idx])

                if not v: # 겹치지 않음
                    # 2: 전체, 1: 상체, 0:unkwon
                    if frame_count == 0:
                        state_bit, total_crop = run_keypoint(frame_bbox, frame_img, keypoint_model, strong=False)
                    else:
                        state_bit, total_crop = run_keypoint(frame_bbox, frame_img, keypoint_model)
                    
                    reid_feature_total = extract_reid_feature(total_crop, reid_model)
                    
                    if state_bit==2: # 다보이는 경우 <type:2>
                        projected_x, projected_y = extract_global_position2(frame_bbox, H_list[idx])           
                        # <channel_track_id> <project_x> <project_y> <reid_feat> <state_bit>
                        local_obj_mapping[idx].append([frame_bbox[5], projected_x, projected_y, reid_feature_total, 2]) 

                    else: # Unkown <type:1,0>
                        local_obj_mapping[idx].append([frame_bbox[5], None, None, reid_feature_total,0])

                else : # 겹침 <type:3>
                    local_obj_mapping[idx].append([frame_bbox[5], None, None, None, 3])
    
    
    
    # print(np.array(local_obj_mapping))
    # local_obj_mapping 객체 중에서 근처로 접근 하는것 같으면 -> 해당 객체는 겹침으로 간주 class 3
    local_obj_mapping = check_occlusion(local_obj_mapping)
    
    # Control type 2
    if frame_count == 0:
        global_obj_mapping, channel_id_mapping, local_channel_loss_mapping, new_id = add_local2gloabl(global_obj_mapping, channel_id_mapping, local_obj_mapping, new_id)
    else:
        global_obj_mapping, channel_id_mapping, local_channel_loss_mapping = match_local2gloabl(global_obj_mapping, channel_id_mapping, local_obj_mapping)
    
    # 2번만
    # online -> type2 번 loss 처리 (실시간) | local_channel_loss_mapping

    global_obj_mapping, channel_id_mapping, new_id = process_online(global_obj_mapping, channel_id_mapping, local_channel_loss_mapping, new_id)

    # local_channel_loss_mapping 
    # channel_loss_mapping
    # local_obj_mapping
    # Format : ([channel_id],[],...) |
    # channel_id ->  <channel_track_id> <project_x> <project_y> <reid_feat> <state_bit>
    

    # local_obj 우선 쓰고
    for i in range (len(channel_final_output)):
        for row in track_result[i]:
            channel_track_id = row[-1]
            # print("채널 아이디",i,"채널 트랙 아이디",channel_track_id)
            
            if channel_track_id in channel_id_mapping[i].keys(): # 매핑 성공 시 add  
                global_track_id = channel_id_mapping[i][channel_track_id]
                channel_final_output[i].append([i, global_track_id , frame_count , row[0] , row[1] , row[2]-row[0] , row[3]-row[1] , 
                                                global_obj_mapping[global_track_id][0], global_obj_mapping[global_track_id][1]])
            else: # 매칭 실패한 현재 프레임 obj
                temp = None
                for obj in local_obj_mapping[i]:
                    if obj[0] == channel_track_id:
                        temp = obj     
                channel_loss_final_output[i].append(([i, None , frame_count , row[0] , row[1] , row[2]-row[0] , row[3]-row[1] , 
                                                    -1, -1], temp)) # <- 추후에 매칭 기다리거나 or 매칭 안되면 offline 매칭 수행
    
    
    # 저장된 loss_channel_mappiing에 대하여 새롭게 찾은게 있나           
    channel_loss_final_output, channel_final_output = process_loss(channel_loss_final_output, channel_final_output, channel_id_mapping)
    
    frame_count+=1
    t.update(1)
    
    # %matplotlib inline
    # plot_site(frame1,frame2,frame3,frame4,frame5,frame6)
    
    # if frame_count>end:
    #     print("stop")
    #     break
    
    _, frame1 = cap1.read()
    _, frame2 = cap2.read()
    _, frame3 = cap3.read()
    _, frame4 = cap4.read()
    _, frame5 = cap5.read()
    _, frame6 = cap6.read()

t.close()

SAVE_PATH = '/data/cvprw/AIC23/tracking/SUBMISSION/S021'

# just save
for idx, channel_result in enumerate(channel_final_output):
    # Open a file for writing
    with open(os.path.join(SAVE_PATH,str(idx)+'_associated.txt'), 'w') as f:
        # Loop through the rows of the array and write them to the file
        for row in channel_result:
            # Convert the row to a string and add a newline character
            row_str = ','.join(str(elem) for elem in row) + '\n'
            f.write(row_str)

# save after sort
# just save
for idx, channel_result in enumerate(channel_final_output):
    sort_indices = np.argsort(np.array(channel_result)[:, 2])
    sorted_channel_result = list(np.array(channel_result)[sort_indices])
    
    # Open a file for writing
    with open(os.path.join(tracking_result_path, channel_list[idx],'sorted_associated.txt'), 'w') as f:
        # Loop through the rows of the array and write them to the file
        for row in sorted_channel_result:
            # Convert the row to a string and add a newline character
            row_str = ','.join(str(elem) for elem in row) + '\n'
            f.write(row_str)


cap1.release()
cap2.release()
cap3.release()
cap4.release()
cap5.release()
cap6.release()