import os
import numpy as np

def checkEncoding(filepath):
    filepath = filepath
    with open(filepath, "rb") as encode_check:
        encoding = encode_check.readline(3)
    if encoding == b"\xfe\xff\x00":
        return "utf_16_be"
    elif encoding == b"\xff\xfe0":
        return "utf_16_le"
    else:
        return "utf_8"

def readTextFile(filepath):
    filepath = filepath
    lines = None
    if os.path.exists(filepath):
        file_encoding = checkEncoding(filepath)
        try:
            with open(filepath, "rt", encoding=file_encoding) as f_in:
                lines = f_in.readlines()
        except:
            with open(filepath, "rt", encoding="latin_1") as f_in:
                lines = f_in.readlines()

    return lines

def apply_rotation(x, y, z, rot_mat):
    # 旋转矩阵乘法: [x', y', z'] = [x, y, z] * rot_mat
    new_x = rot_mat[0][0] * x + rot_mat[0][1] * y + rot_mat[0][2] * z
    new_y = rot_mat[1][0] * x + rot_mat[1][1] * y + rot_mat[1][2] * z
    new_z = rot_mat[2][0] * x + rot_mat[2][1] * y + rot_mat[2][2] * z
    return new_x, new_y, new_z


def apply_rotation_matrix(orig_mat, rot_mat):
    # numpy 矩阵乘法
    orig_mat_np = np.array(orig_mat).reshape((3,3))
    rot_mat_np = np.array(rot_mat)
    new_mat = rot_mat_np @ orig_mat_np
    return new_mat.flatten().tolist()

def getModulePart(start_line, end_line, lines, main_section, rot_mat):
    for j in range(start_line, end_line):
        line = lines[j]
        if line.startswith("1 "):
            parts = line.strip().split()
            if len(parts) >= 12:  # x,y,z + 9 个矩阵元素
                try:
                    # 获取原坐标
                    x = float(parts[2])
                    y = float(parts[3])
                    z = float(parts[4])
                    
                    # 使用旋转矩阵进行坐标变换
                    x_rot, y_rot, z_rot = apply_rotation(x, y, z, rot_mat)
                    
                    # 更新坐标
                    parts[2] = str(x_rot)
                    parts[3] = str(y_rot)
                    parts[4] = str(z_rot)
                    
                    # 获取原旋转矩阵
                    orig_mat = [float(p) for p in parts[5:14]]
                    
                    # 旋转零件自身旋转矩阵
                    new_mat = apply_rotation_matrix(orig_mat, rot_mat)
                    
                    # 更新矩阵
                    for idx, val in enumerate(new_mat):
                        parts[5 + idx] = str(val)
                    
                    # 重新拼接并更新行
                    line = " ".join(parts) + "\n"
                except ValueError as e:
                    print(e)
                    pass
        main_section.append(line)



def process_ldr(filepath, output_file, rot_mat):
    if os.path.isfile(filepath):
        lines = readTextFile(filepath)
        if lines is None:
            print("Could not read file {0}".format(filepath))
            lines = []
    else:
        lines = []

    main_section = []
    startLine = 0
    endLine = 0
    lineCount = 0
    foundEnd = False
    
    for line in lines:
        parameters = line.strip().split()
        if len(parameters) > 2:
            if parameters[0] == "0" and parameters[1] == "FILE":
                if foundEnd == False:
                    endLine = lineCount
                    if endLine > startLine:
                        getModulePart(startLine, endLine, lines, main_section, rot_mat)
                        foundEnd = True
                        break

                startLine = lineCount
                foundEnd = False

            if parameters[0] == "0" and parameters[1] == "NOFILE":
                endLine = lineCount
                foundEnd = True
                getModulePart(startLine, endLine, lines, main_section, rot_mat)
                break
        lineCount += 1

    if lineCount < len(lines):
        remaining = lines[lineCount:]
    else:
        remaining = []

    if foundEnd == False:
        endLine = lineCount
        if endLine > startLine:
            getModulePart(startLine, endLine, lines, main_section, rot_mat)

    new_section = main_section + remaining
    with open(output_file, "w", encoding="utf-8") as f:
        f.writelines(new_section)
        return True

# 旋转矩阵：绕 Y 轴旋转 90 度
rot_mat = [
    [0, 0, 1],
    [0, 1, 0],
    [-1, 0, 0]
]

# # 调用函数处理 LDR 文件，进行旋转
# def main():
#     process_ldr("/public/home/wangshuo/gap/assembly/data/car_1k/ldr/car_5/car_5.ldr", "car_5_rotate.ldr", rot_mat)

# main()