import cv2
print(cv2.__version__) # 4.9.0
import json
import math
import copy
import numpy as np

def get_params(num_param, start, end):
    return copy.deepcopy(num_param[start:end+1])

def pramas_to_num(text_param):
    for item in text_param:
        item['center'] = float(item['center']) * 1000
        item['x'] = int(item['x'])
        item['w'] = int(item['w'])
        item['h'] = int(item['h'])
    return text_param

def pramas_to_text(num_param):
    for item in num_param:
        item['center'] = str(item['center'])
        item['x'] = str(item['x'])
        item['w'] = str(item['w'])
        item['h'] = str(item['h'])
        item['x1'] = str(item['x1'])
        item['y1'] = str(item['y1'])
        item['x2'] = str(item['x2'])
        item['y2'] = str(item['y2'])
        item['x3'] = str(item['x3'])
        item['y3'] = str(item['y3'])
        item['x4'] = str(item['x4'])
        item['y4'] = str(item['y4'])
    return num_param

def sort_params(params):
    sorted_params = sorted(params, key=lambda item: (item['center'], item['x']))
    return sorted_params

def print_params(sort_params):
    for param in sort_params:
        print(param["center"], param["x"], param["w"], param["h"])

def print_path(search_path):
    for path in search_path:
        print(path[0], path[1])

def search_path(sort_params):
    searchPath = []
    for i in range(len(sort_params) - 1):
        (r, theta) = cartesian_to_polar(sort_params[i]["x"], sort_params[i]["center"], 
                        sort_params[i + 1]["x"], sort_params[i + 1]["center"])
        searchPath.append([r, theta])
    return searchPath

def normalize_params_and_path(sort_params, search_path, index=0):
    base = sort_params[index]["h"] 
    for param in sort_params:
        param['center'] /= base
        param['x'] /= base
        param['w'] /= base
        param['h'] /= base
        param['w/h'] = param['w'] / param['h']
    if search_path != None:
        for path in search_path:
            path[0] /= base
            # path[1] /= base
    return sort_params, search_path

def read_from_json(file_path):
    with open(file_path, 'r') as f:
        loaded_array = json.load(f)
    return loaded_array

def cartesian_to_polar(x1, y1, x2, y2):
    dx = x2 - x1
    dy = y2 - y1
    r = math.sqrt(dx**2 + dy**2)
    theta = math.atan2(dy, dx)
    return r, theta

def calculate_second_point(x1, y1, r, theta_radians):
    # theta_radians = math.radians(theta_degrees)
    x2 = x1 + r * math.cos(theta_radians)
    y2 = y1 + r * math.sin(theta_radians)
    
    return x2, y2

def filter_params(data_bim_candi, search_path, sort_params):
    loop = 0
    for path in search_path:
        r = path[0]
        theta = path[1]
        cur_parmas = sort_params[loop]
        next_params = sort_params[loop + 1]
        exclude_list = []
        for i in range(len(data_bim_candi)):
            param = data_bim_candi[i]["params"][data_bim_candi[i]["index"]]
            center = param["center"]
            x = param["x"]
            h = param["h"]
            w = param["w"]
            hmw = param["w/h"]
            # 长宽比大于1.5或小于0.67，则配出当前选项
            if hmw / cur_parmas["w/h"]  > 1.5 or hmw / cur_parmas["w/h"] < 0.67:
                exclude_list.append(data_bim_candi[i])
                break
            # 判断下一跳位置是否击中目标
            tmp_x, tmp_y = calculate_second_point(x, center, r, theta)
            find = False
            for j in range(len(data_bim_candi)):
                tmp_param = data_bim_candi[i]["params"][j]
                tmp_x = tmp_param["x"]
                tmp_y = tmp_param["center"]
                tmp_distence = np.linalg.norm(np.asarray([tmp_x, tmp_y]) - np.asarray([x, center]))
                if abs(tmp_distence - r) < r / 10:
                    data_bim_candi[i]["index"] = 
                    break
            if not find:
                exclude_list.append(data_bim_candi[i])
            

            print(data_bim_candi[i]["params"][i])
        loop += 1

if __name__ == "__main__":

    # im_bim = cv2.imread("")
    # im_sub = cv2.imread("")
    data_bim = {}
    data_bim["type"] = 0
    data_bim["params"] = read_from_json("./params.json")
    data_bim["point"] = []

    data_sub_0_4 = {}
    data_sub_0_4["type"] = 0
    data_sub_0_4["params"] = get_params(data_bim["params"], 0, 4)
    data_sub_0_4["point"] = []

    data_sub_4_9 = {}
    data_sub_4_9["type"] = 0
    data_sub_4_9["params"] = get_params(data_bim["params"], 4, 9)
    data_sub_4_9["point"] = []

    data_sub_10_14 = {}
    data_sub_10_14["type"] = 0
    data_sub_10_14["params"] = get_params(data_bim["params"], 10, 14)
    data_sub_10_14["point"] = []
    print(data_sub_10_14["params"])

    ##################### 开始计算 ####################
    # 1、计算sub的搜索路径
    ## 1.1 排序 y升序，x升序
    data_bim["params"] = pramas_to_num(data_bim["params"])
    data_sub = data_sub_0_4
    data_sub["params"] = pramas_to_num(data_sub["params"])
    data_sub["params"] = sort_params(data_sub["params"])
    _sort_params = data_sub["params"]

    ## 1.2生成路径数据
    _search_path = search_path(_sort_params)
    # print(_search_path)

    ## 1.3 归一化 sort_params 和 searchPath
    _sort_params, _search_path = normalize_params_and_path(_sort_params, _search_path)
    print_params(_sort_params)
    print_path(_search_path)

    # 2、开始n轮bim图的搜索，首轮全部数据参与计算，每轮淘汰若干选项，直到整个路径搜索完毕，剩下的选项都是候选目标区域
    ## 2.1 给bim所有的预埋件都分配一个完整的图
    num = len(data_bim["params"])
    _data_bim_candi = []
    for i in range(num):
        data_bim_copy = {}
        data_bim_copy["params"] = get_params(data_bim["params"], 0, num-1)
        data_bim_copy["index"] = i
        _sort_params, _ = normalize_params_and_path(data_bim_copy["params"], None, i) # 用第i个件作为归一化指标
        _data_bim_candi.append(data_bim_copy)
    ## 2.2 判断 data_sub["params"] w 和 h 的比值
    filter_params(_data_bim_candi, _search_path, _sort_params)