import socket
import json
import os
import math
import numpy as np
import time

from typing import Literal
from pprint import pprint

import threading
import pybullet as p
import pybullet_industrial as pi

from logger import logger

class UrdfManager:
    urdf_filename = None
    physics_client = None
    body_id = None

    def __init__(self, robot_start_position, application_path):
        self.robot_start_position = robot_start_position
        self.application_path = application_path

    def start_loop(self, urdf):
        self.urdf_filename = urdf

        p.resetSimulation()
        self.load_models()

    def run_pybullet(self, type="DIRECT"):
        self.physics_client = p.connect(getattr(p, type))
        logger.info(f"Connect to {self.physics_client} by {type}")

    def load_models(self):
        # p.loadURDF("urdf/plane.urdf", physicsClientId=self.physics_client)

        urdf_path = os.path.join(
            self.application_path, "urdf", f"{self.urdf_filename}.urdf"
        )
        self.body_id = p.loadURDF(urdf_path, self.robot_start_position)

        time.sleep(1)

    def get_pybullet_image(self):
        if self.physics_client is None:
            return

        width, height, rgb, _, _ = p.getCameraImage(
            width=500,
            height=500,
            viewMatrix=p.computeViewMatrix(
                cameraEyePosition=[4, -4, 1.5],
                # cameraEyePosition=[4,-4,4],
                cameraTargetPosition=[0, 0, 0],  # Центр фокусировки камеры
                cameraUpVector=[0, 0, 1],  # Направление вверх
            ),
            projectionMatrix=p.computeProjectionMatrixFOV(
                fov=60.0, aspect=1.0, nearVal=0.1, farVal=10.0
            ),
            # renderer=p.ER_TINY_RENDERER,
            physicsClientId=self.physics_client,
        )

        return (rgb, width, height)