import numpy as np
import json
from typing import Dict, Any

class CameraModel:
    def __init__(self, config_path: str = None):
        """
        初始化相机模型，支持从配置文件加载参数。
        :param config_path: 配置文件的路径（JSON/YAML）。如果为 None，则使用默认参数。
        """
        # 默认参数
        self._default_params = {
            "K":       [[838.19297892,   0,         658.51580973],
                            [0,         836.93529706, 960-484.42225009],
                            [0,           0,           1]],
            "dist_coeffs": [-0.450688803, 0.260760637, 0.000210619624, -0.000669674309, -0.0447846385],
            "focal_length": 3.6,
            "height": 1007,  # 1.007e+03 - 9
            "rotation_alpha": 0.9135,
            "rotation_beta": 0.7271,
            "position_offset_x": 0,
            "position_offset_y": 53,
            "rotation_camera": -0.02892
        }

        # 如果提供了配置文件，则从文件加载参数
        if config_path:
            self.load_config(config_path)
        else:
            self._set_parameters(self._default_params)

    def _set_parameters(self, params: Dict[str, Any]) -> None:
        """根据参数字典设置成员变量"""
        self.K = np.array(params["K"])
        self.dist_coeffs = np.array(params["dist_coeffs"])
        self.focal_length = params["focal_length"]
        self.height = params["height"]
        self.rotation_alpha = params["rotation_alpha"]
        self.rotation_beta = params["rotation_beta"]
        self.position_offset_x = params["position_offset_x"]
        self.position_offset_y = params["position_offset_y"]
        self.rotation_camera = params["rotation_camera"]

        # 计算派生属性
        self.pixel_size_x = self.focal_length / self.K[0, 0]
        self.pixel_size_y = self.focal_length / self.K[1, 1]
        self.principal_point = (self.K[0, 2], self.K[1, 2])

    def load_config(self, file_path: str) -> None:
        """从 JSON 文件加载参数"""
        with open(file_path, 'r') as f:
            params = json.load(f)
        self._set_parameters(params)

    def save_config(self, file_path: str) -> None:
        """将当前参数保存到 JSON 文件"""
        params = {
            "K": self.K.tolist(),
            "dist_coeffs": self.dist_coeffs.tolist(),
            "focal_length": self.focal_length,
            "height": self.height,
            "rotation_alpha": self.rotation_alpha,
            "rotation_beta": self.rotation_beta,
            "position_offset_x": self.position_offset_x,
            "position_offset_y": self.position_offset_y,
            "rotation_camera": self.rotation_camera
        }
        with open(file_path, 'w') as f:
            json.dump(params, f, indent=4)

    def update_parameter(self, key: str, value: Any) -> None:
        """动态更新单个参数"""
        if hasattr(self, key):
            setattr(self, key, value)
            # 如果修改了 K 或焦距，需要重新计算派生属性
            if key in ["K", "focal_length"]:
                self.pixel_size_x = self.focal_length / self.K[0, 0]
                self.pixel_size_y = self.focal_length / self.K[1, 1]
                self.principal_point = (self.K[0, 2], self.K[1, 2])
        else:
            raise AttributeError(f"参数 {key} 不存在")

model = CameraModel(None)

# model.save_config("updated_config.json")

# # 修改单个参数
# model.update_parameter("focal_length", 4.0)
#
# # 直接修改内参矩阵
# model.K = np.array([[900, 0, 350], [0, 900, 280], [0, 0, 1]])
# model.update_parameter("K", model.K)  # 触发派生属性重新计算
# print(f"model: {model.K}")
model.save_config("updated_config.json")