# -- coding: utf-8 --
import inspect
import logging
import logging.config
import numpy as np
import os
import json
import numpy as np
from scipy.spatial.transform import Rotation as R
import cv2
import config
import utils
import svd
from scipy.spatial import KDTree
# import matplotlib.pyplot as plt

if __name__ == "__main__":

    all_points = utils.read_from_json(os.path.dirname(__file__) + '\\record\\points.json')
    records = utils.read_from_json(os.path.dirname(__file__) + '\\record\\record.json')
    aurco_marks_conners_all = all_points[0]
    pcd_marks_conners_all = all_points[1]
   
    # print(aurco_marks_conners_all)
    # print(pcd_marks_conners_all)

    _R, _t, _, _ = svd.scipy_svd(
        np.asarray(pcd_marks_conners_all), 
        np.asarray(aurco_marks_conners_all))
    _r, _p, _y = utils.rotation_matrix_to_rpy(_R)
    # 计算从初始值到最终rt的结果
    _new_rpy = utils.combine_rpy(config.roll, config.pitch, config.yaw, _r, _p, _y)
    # print(_R)
    print(_new_rpy)
    print(_t)

    # 优化 RT
    # RT_matrices = []
    # for i in range(len(records)):
    #     record = records[i]
    #     _r, _p, _y = record[4]
    #     _t = np.asarray(record[3])
    #     _R = utils.rpy_to_rotation_matrix(_r, _p, _y)
    #     RT_matrices.append([_R, _t])
    # R_opt, t_opt = utils.filter_and_fit_rts(
    #     RT_matrices, np.asarray(pcd_marks_conners_all), np.asarray(aurco_marks_conners_all),
    #     threshold=0.001, use_ransac=True)
    # _r, _p, _y = utils.rotation_matrix_to_rpy(R_opt)
    # _new_rpy = utils.combine_rpy(config.roll, config.pitch, config.yaw, _r, _p, _y)
    # print(_new_rpy)
    # print(t_opt)

    # _pdc_rt = (R_opt @ np.asarray(pcd_marks_conners_all).T).T + t_opt

    _pdc_rt = (_R @ np.asarray(pcd_marks_conners_all).T).T + _t

    distances = []
    cnt = 0
    _distances = 0
    for i in range(len(_pdc_rt)):
        # print(np.linalg.norm(_pdc_rt[i] - aurco_marks_conners_all[i]))
        temp_distanece = np.linalg.norm(_pdc_rt[i] - aurco_marks_conners_all[i])
        distances.append(temp_distanece)
        _distances += temp_distanece
        cnt += 1

    # print(distances)
    print("均值:", _distances/cnt)
    # plt.plot(distances)
    # plt.show()