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 = {
            'camera_width': 1280,
            'camera_height': 960,
            "origin_K":     np.array([[1086.58411235010, 0.00000000e+00, 656.379279306796],
                                      [0.00000000e+00, 1086.65442545872, 960-489.101890262552],
                                      [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
            "undistor_K":       np.array([[838.19297892,   0,         658.51580973],
                                          [0,         836.93529706, 960-484.42225009],
                                          [0,           0,           1]]),
            "dist_coeffs": np.array([-0.450688803, 0.260760637, 0.000210619624, -0.000669674309, -0.0447846385]),
            "focal_length": 3.6,
            "height": 1007.0633042608584,  # 1.007e+03 - 9
            "rotation_alpha": 0.8892272342384955,
            "rotation_beta": 0.6786627929104145,
            "position_offset_x": 507,
            "position_offset_y": 32,
            "rotation_camera": 0.03985022053087201,

            "filter": True,
            "filt_percent": 0.1,
            "outlier_num": 10,
            "ransac_residual_threshold": 2.5,
            "grid_downsample": True,
            "cell_size": 20,

        }

        # 如果提供了配置文件，则从文件加载参数
        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.camera_width = params["camera_width"]
        self.camera_height = params["camera_height"]
        self.origin_K = np.array(params["origin_K"])
        self.undistor_K = np.array(params["undistor_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.filter = params["filter"]
        self.filt_percent = params["filt_percent"]
        self.outlier_num = params["outlier_num"]
        self.ransac_residual_threshold = params["ransac_residual_threshold"]
        self.grid_downsample = params["grid_downsample"]
        self.cell_size = params["cell_size"]
        # 计算派生属性
        self.pixel_size_x = self.focal_length / self.undistor_K[0, 0]
        self.pixel_size_y = self.focal_length / self.undistor_K[1, 1]
        self.principal_point = (self.undistor_K[0, 2], self.undistor_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 = {
            "camera_width": self.camera_width,
            "camera_height": self.camera_height,
            "origin_K": self.origin_K.tolist(),
            "undistor_K": self.undistor_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,
            "filter": self.filter,
            "filt_percent": self.filt_percent,
            "outlier_num": self.outlier_num,
            "grid_downsample": self.grid_downsample,
            "cell_size": self.cell_size,
            "ransac_residual_threshold": self.ransac_residual_threshold
        }
        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 ["undistor_K", "focal_length"]:
                self.pixel_size_x = self.focal_length / self.undistor_K[0, 0]
                self.pixel_size_y = self.focal_length / self.undistor_K[1, 1]
                self.principal_point = (self.undistor_K[0, 2], self.undistor_K[1, 2])
        else:
            raise AttributeError(f"参数 {key} 不存在")

model = CameraModel(None)
model.save_config("updated_config.json")