import os
import pybullet as p
import pybullet_data
import numpy as np
from gym import spaces
from util.color_print import printGreen, printBlue, printRed, printYellow
import gym
from environments.inmoov import inmoov
from environments.inmoov.joints_registry import joint_registry
from environments.inmoov.joints_registry import zhuaqu
from environments.srl_env import SRLGymEnv
GRAVITY = -9.8
URDF_PATH = "/urdf_robot/"
RENDER_WIDTH, RENDER_HEIGHT = 224, 224


# python -m rl_baselines.train --env InmoovGymEnv-v0 --srl-model ground_truth --algo ppo2 --log-dir logs/
# --num-timesteps 200000

def getGlobals():
    """
    :return: (dict)
    """
    return globals()


class InmoovGymEnv(SRLGymEnv):
    def __init__(self, urdf_path=URDF_PATH, max_steps=1000,
                 env_rank=0,
                 srl_pipe=None,
                 action_repeat=1, srl_model="ground_truth",
                 seed=0, debug_mode=False, **kwargs):
        """

        :param urdf_path:
        :param max_steps:
        :param env_rank:
        :param srl_pipe:
        :param action_repeat:
        :param srl_model:
        :param seed: (int) the random seed for the GymEnv
        :param debug_mode: (bool) if True, the GUI will show up during the training (or for dug purpose)
        :param kwargs:
        """
        super(InmoovGymEnv, self).__init__(srl_model=srl_model,
                                           relative_pos=True,
                                           env_rank=env_rank,
                                           srl_pipe=srl_pipe)

        self.seed(seed)
        self.urdf_path = urdf_path

        self._observation = None
        self.debug_mode = debug_mode
        self._inmoov = None
        self._observation = None
        self.action_repeat = action_repeat
        self._inmoov_id = -1
        self._tomato_id = -1
        self.max_steps = max_steps
        self._step_counter = 0
        self._render = False
        # for more information, please refer to the function _get_tomato_pos
        self._tomato_link_id = 3
        self.joints_key = None
        self.srl_model = srl_model
        self.camera_target_pos = (0.0, 0.0, 1.0)
        self._width = RENDER_WIDTH
        self._height = RENDER_HEIGHT
        self.terminated = False
        self.n_contacts = 0
        self.state_dim = self.getGroundTruthDim()
        self._first_reset_flag = False

        if debug_mode:
            client_id = p.connect(p.SHARED_MEMORY)
            if client_id < 0:
                p.connect(p.GUI)
            p.resetDebugVisualizerCamera(5., 180, -41, [0.52, -0.2, -0.33])
            debug_joints = []
            self.joints_key = []
            for joint_index in joint_registry:
                self.joints_key.append(joint_index)
                # This will add some slider on the GUI
                debug_joints.append(p.addUserDebugParameter(joint_registry[joint_index], -1., 1., 0))

            self.debug_joints = debug_joints
        else:
            p.connect(p.DIRECT)

        # TODO: here, we only use, for the moment, discrete action
        self.action_space = spaces.Discrete(8)
        if self.srl_model == "raw_pixels":
            self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 3), dtype=np.uint8)
        else:  # Todo: the only possible observation for srl_model is ground truth
            self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(3,), dtype=np.float32)
        self.reset()

    def reset(self):
        self._step_counter = 0
        self.terminated = False
        self.n_contacts = 0
        if not self._first_reset_flag:
            # print('first reset,loading urdf...')
            p.resetSimulation()
            self._first_reset_flag = True
            p.setPhysicsEngineParameter(numSolverIterations=150)
            p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0])
            p.setGravity(0, 0, GRAVITY)

            self._inmoov = inmoov.Inmoov(urdf_path=self.urdf_path, positional_control=True)
            self._inmoov_id = self._inmoov.inmoov_id

            self._tomato_id = p.loadURDF(os.path.join(self.urdf_path, "tomato_plant.urdf"), [0.4, 0.4, 0.5])
        print('fast reset,resetting robot joints')
        self._inmoov.reset_joints()
        # p.resetSimulation()
        # p.setPhysicsEngineParameter(numSolverIterations=150)
        # p.setGravity(0., 0., GRAVITY)
        return self.get_observation()

    def _get_effector_pos(self):
        return self._inmoov.getGroundTruth()

    def _get_tomato_pos(self):
        # 0: the tomato at the bottom
        # 1,2 : the two little tomatoes on top
        # 3: the only lonely tomato on the top
        tomato_pos = p.getLinkState(self._tomato_id, self._tomato_link_id)[0]
        return np.array(tomato_pos)

    def ground_truth(self):
        # a relative position
        return self._get_effector_pos() - self._get_tomato_pos()

    def getSRLState(self, observation=None):
        # TODO: raw pixels
        if self.srl_model == "ground_truth":
            return self.ground_truth()

    def getGroundTruth(self):
        return np.array(self._get_effector_pos())

    def getTargetPos(self):
        return self._get_tomato_pos()

    @staticmethod
    def getGroundTruthDim():
        return 3

    def _reward(self):
        distance = np.linalg.norm(self._get_effector_pos() - self._get_tomato_pos(), 2)
        re = -distance 
        if distance < 0.1:
            re = 0
        for id in zhuaqu:
            re += len(p.getContactPoints(self._inmoov_id, self._tomato_id, id))
        # printYellow("The distance between target and effector: {:.2f}".format(distance))
        return re

    def _termination(self):
        """
        :return: (bool) whether an episode is terminated
        """
        if self.terminated or self._step_counter > self.max_steps:
            return True
        return False

    def effector_position(self):
        return self._inmoov.getGroundTruth()

    def guided_step(self):
        """
        Effect a guided step towards the target
        :return:
        """
        tomato_pos = self._get_tomato_pos()
        #eff_pos = self._get_effector_pos()
        eff_pos = p.getLinkState(self._inmoov_id, 37)[0]
        action = tomato_pos - eff_pos  
        print(np.linalg.norm(action, 2))
        self._inmoov.apply_action_pos(action)
        if np.linalg.norm(action, 2) < 0.1:
            self._inmoov.zhua()

    def server_step(self):
        raise NotImplementedError

    def step(self, action):
        # TODO: bug might be here
        # if action == 5:
        #     self.guided_step()
        if action is None:
            action = np.array([0, 0, 0])
        dv = 0.1
        dx = [-dv, dv, 0, 0, 0, 0,0,0][action]
        dy = [0, 0, -dv, dv, 0, 0,0,0][action]
        dz = [0, 0, 0, 0, -dv, dv,0,0][action]
        if action == 7:
            self._inmoov.zhua()
        if action == 6:
            self._inmoov.song()
        #action = [dx, dy, dz]+np.random.normal(size=3, scale=1/30, loc=0)
        action = [dx, dy, dz]
        for i in range(self.action_repeat):
            self._inmoov.apply_action_pos(action)
            p.stepSimulation()
        self._step_counter += 1
        reward = self._reward()
        obs = self.get_observation()
        done = self._termination()
        infos = {}
        if self._render and self.debug_mode:
            self.render()
        return np.array(obs), reward, done, infos
        # printGreen(action)
        # printYellow(self._inmoov.getGroundTruth())
        # tt()
        # self._observation = self.render(mode='rgb')
        # reward = self._reward()
        # done = self._termination()
        # self.obs[:], rewards, self.dones, infos
        # return np.array(self._observation), reward, done, {}

    def get_observation(self):
        if self.srl_model == "raw_pixels":
            self._observation = self.render(mode="rgb")
            return self._observation
        elif self.srl_model == "ground_truth":
            self._observation = self.ground_truth()
            return self.ground_truth()
        else:
            raise NotImplementedError


    def render(self, mode='rgb'):
        """
        return the RBG image
        :param mode: 'robot' to get the robot view
        :return:
        """
        camera_target_position = self.camera_target_pos
        view_matrix1 = p.computeViewMatrixFromYawPitchRoll(
            cameraTargetPosition=camera_target_position,
            distance=2.,
            yaw=145,  # 145 degree
            pitch=-36,  # -36 degree
            roll=0,
            upAxisIndex=2
        )
        proj_matrix1 = p.computeProjectionMatrixFOV(
            fov=60, aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
            nearVal=0.1, farVal=100.0)
        (_, _, px, depth, mask) = p.getCameraImage(
            width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix1,
            projectionMatrix=proj_matrix1, renderer=p.ER_TINY_RENDERER)
        px, depth, mask = np.array(px), np.array(depth), np.array(mask)
        px = px.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        depth = depth.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        mask = mask.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        px = px[..., :-1]


        return px
    
    def close(self):
        return
   

import os
import pybullet as p
import pybullet_data
import numpy as np
from gym import spaces
from util.color_print import printGreen, printBlue, printRed, printYellow
import gym
from environments.inmoov import inmoov
from environments.inmoov.joints_registry import joint_registry
from environments.inmoov.joints_registry import zhuaqu
from environments.srl_env import SRLGymEnv
GRAVITY = -9.8
URDF_PATH = "/urdf_robot/"
RENDER_WIDTH, RENDER_HEIGHT = 224, 224


# python -m rl_baselines.train --env InmoovGymEnv-v0 --srl-model ground_truth --algo ppo2 --log-dir logs/
# --num-timesteps 200000

def getGlobals():
    """
    :return: (dict)
    """
    return globals()


class InmoovGymEnv(SRLGymEnv):
    def __init__(self, urdf_path=URDF_PATH, max_steps=1000,
                 env_rank=0,
                 srl_pipe=None,
                 action_repeat=1, srl_model="ground_truth",
                 seed=0, debug_mode=False, **kwargs):
        """

        :param urdf_path:
        :param max_steps:
        :param env_rank:
        :param srl_pipe:
        :param action_repeat:
        :param srl_model:
        :param seed: (int) the random seed for the GymEnv
        :param debug_mode: (bool) if True, the GUI will show up during the training (or for dug purpose)
        :param kwargs:
        """
        super(InmoovGymEnv, self).__init__(srl_model=srl_model,
                                           relative_pos=True,
                                           env_rank=env_rank,
                                           srl_pipe=srl_pipe)

        self.seed(seed)
        self.urdf_path = urdf_path

        self._observation = None
        self.debug_mode = debug_mode
        self._inmoov = None
        self._observation = None
        self.action_repeat = action_repeat
        self._inmoov_id = -1
        self._tomato_id = -1
        self.max_steps = max_steps
        self._step_counter = 0
        self._render = False
        # for more information, please refer to the function _get_tomato_pos
        self._tomato_link_id = 3
        self.joints_key = None
        self.srl_model = srl_model
        self.camera_target_pos = (0.0, 0.0, 1.0)
        self._width = RENDER_WIDTH
        self._height = RENDER_HEIGHT
        self.terminated = False
        self.n_contacts = 0
        self.state_dim = self.getGroundTruthDim()
        self._first_reset_flag = False

        if debug_mode:
            client_id = p.connect(p.SHARED_MEMORY)
            if client_id < 0:
                p.connect(p.GUI)
            p.resetDebugVisualizerCamera(5., 180, -41, [0.52, -0.2, -0.33])
            debug_joints = []
            self.joints_key = []
            for joint_index in joint_registry:
                self.joints_key.append(joint_index)
                # This will add some slider on the GUI
                debug_joints.append(p.addUserDebugParameter(joint_registry[joint_index], -1., 1., 0))

            self.debug_joints = debug_joints
        else:
            p.connect(p.DIRECT)

        # TODO: here, we only use, for the moment, discrete action
        self.action_space = spaces.Discrete(8)
        if self.srl_model == "raw_pixels":
            self.observation_space = spaces.Box(low=0, high=255, shape=(self._height, self._width, 3), dtype=np.uint8)
        else:  # Todo: the only possible observation for srl_model is ground truth
            self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(3,), dtype=np.float32)
        self.reset()

    def reset(self):
        self._step_counter = 0
        self.terminated = False
        self.n_contacts = 0
        if not self._first_reset_flag:
            # print('first reset,loading urdf...')
            p.resetSimulation()
            self._first_reset_flag = True
            p.setPhysicsEngineParameter(numSolverIterations=150)
            p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0])
            p.setGravity(0, 0, GRAVITY)

            self._inmoov = inmoov.Inmoov(urdf_path=self.urdf_path, positional_control=True)
            self._inmoov_id = self._inmoov.inmoov_id

            self._tomato_id = p.loadURDF(os.path.join(self.urdf_path, "tomato_plant.urdf"), [0.4, 0.4, 0.5])
        print('fast reset,resetting robot joints')
        self._inmoov.reset_joints()
        # p.resetSimulation()
        # p.setPhysicsEngineParameter(numSolverIterations=150)
        # p.setGravity(0., 0., GRAVITY)
        return self.get_observation()

    def _get_effector_pos(self):
        return self._inmoov.getGroundTruth()

    def _get_tomato_pos(self):
        # 0: the tomato at the bottom
        # 1,2 : the two little tomatoes on top
        # 3: the only lonely tomato on the top
        tomato_pos = p.getLinkState(self._tomato_id, self._tomato_link_id)[0]
        return np.array(tomato_pos)

    def ground_truth(self):
        # a relative position
        return self._get_effector_pos() - self._get_tomato_pos()

    def getSRLState(self, observation=None):
        # TODO: raw pixels
        if self.srl_model == "ground_truth":
            return self.ground_truth()

    def getGroundTruth(self):
        return np.array(self._get_effector_pos())

    def getTargetPos(self):
        return self._get_tomato_pos()

    @staticmethod
    def getGroundTruthDim():
        return 3

    def _reward(self):
        distance = np.linalg.norm(self._get_effector_pos() - self._get_tomato_pos(), 2)
        re = -distance 
        if distance < 0.1:
            re = 0
        for id in zhuaqu:
            re += len(p.getContactPoints(self._inmoov_id, self._tomato_id, id))
        # printYellow("The distance between target and effector: {:.2f}".format(distance))
        return re

    def _termination(self):
        """
        :return: (bool) whether an episode is terminated
        """
        if self.terminated or self._step_counter > self.max_steps:
            return True
        return False

    def effector_position(self):
        return self._inmoov.getGroundTruth()

    def guided_step(self):
        """
        Effect a guided step towards the target
        :return:
        """
        tomato_pos = self._get_tomato_pos()
        #eff_pos = self._get_effector_pos()
        eff_pos = p.getLinkState(self._inmoov_id, 37)[0]
        action = tomato_pos - eff_pos  
        print(np.linalg.norm(action, 2))
        self._inmoov.apply_action_pos(action)
        if np.linalg.norm(action, 2) < 0.1:
            self._inmoov.zhua()

    def server_step(self):
        raise NotImplementedError

    def step(self, action):
        # TODO: bug might be here
        # if action == 5:
        #     self.guided_step()
        if action is None:
            action = np.array([0, 0, 0])
        dv = 0.1
        dx = [-dv, dv, 0, 0, 0, 0,0,0][action]
        dy = [0, 0, -dv, dv, 0, 0,0,0][action]
        dz = [0, 0, 0, 0, -dv, dv,0,0][action]
        if action == 7:
            self._inmoov.zhua()
        if action == 6:
            self._inmoov.song()
        #action = [dx, dy, dz]+np.random.normal(size=3, scale=1/30, loc=0)
        action = [dx, dy, dz]
        for i in range(self.action_repeat):
            self._inmoov.apply_action_pos(action)
            p.stepSimulation()
        self._step_counter += 1
        reward = self._reward()
        obs = self.get_observation()
        done = self._termination()
        infos = {}
        if self._render and self.debug_mode:
            self.render()
        return np.array(obs), reward, done, infos
        # printGreen(action)
        # printYellow(self._inmoov.getGroundTruth())
        # tt()
        # self._observation = self.render(mode='rgb')
        # reward = self._reward()
        # done = self._termination()
        # self.obs[:], rewards, self.dones, infos
        # return np.array(self._observation), reward, done, {}

    def get_observation(self):
        if self.srl_model == "raw_pixels":
            self._observation = self.render(mode="rgb")
            return self._observation
        elif self.srl_model == "ground_truth":
            self._observation = self.ground_truth()
            return self.ground_truth()
        else:
            raise NotImplementedError


    def render(self, mode='rgb'):
        """
        return the RBG image
        :param mode: 'robot' to get the robot view
        :return:
        """
        camera_target_position = self.camera_target_pos
        view_matrix1 = p.computeViewMatrixFromYawPitchRoll(
            cameraTargetPosition=camera_target_position,
            distance=2.,
            yaw=145,  # 145 degree
            pitch=-36,  # -36 degree
            roll=0,
            upAxisIndex=2
        )
        proj_matrix1 = p.computeProjectionMatrixFOV(
            fov=60, aspect=float(RENDER_WIDTH) / RENDER_HEIGHT,
            nearVal=0.1, farVal=100.0)
        (_, _, px, depth, mask) = p.getCameraImage(
            width=RENDER_WIDTH, height=RENDER_HEIGHT, viewMatrix=view_matrix1,
            projectionMatrix=proj_matrix1, renderer=p.ER_TINY_RENDERER)
        px, depth, mask = np.array(px), np.array(depth), np.array(mask)
        px = px.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        depth = depth.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        mask = mask.reshape(RENDER_HEIGHT, RENDER_WIDTH, -1)
        px = px[..., :-1]


        return px
    
    def close(self):
        return
   

# python -m rl_baselines.train --env InmoovGymEnv-v0 --srl-model ground_truth --algo ppo2 --log-dir logs/ # --num-timesteps 200000 :return: (dict) :param urdf_path:         :param max_steps:         :param env_rank:         :param srl_pipe:         :param action_repeat:         :param srl_model:         :param seed: (int) the random seed for the GymEnv         :param debug_mode: (bool) if True, the GUI will show up during the training (or for dug purpose)         :param kwargs: # for more information, please refer to the function _get_tomato_pos # This will add some slider on the GUI # TODO: here, we only use, for the moment, discrete action # Todo: the only possible observation for srl_model is ground truth # print('first reset,loading urdf...') # p.resetSimulation() # p.setPhysicsEngineParameter(numSolverIterations=150) # p.setGravity(0., 0., GRAVITY) # 0: the tomato at the bottom # 1,2 : the two little tomatoes on top # 3: the only lonely tomato on the top # a relative position # TODO: raw pixels # printYellow("The distance between target and effector: {:.2f}".format(distance)) :return: (bool) whether an episode is terminated Effect a guided step towards the target         :return: #eff_pos = self._get_effector_pos() # TODO: bug might be here # if action == 5: #     self.guided_step() #action = [dx, dy, dz]+np.random.normal(size=3, scale=1/30, loc=0) # printGreen(action) # printYellow(self._inmoov.getGroundTruth()) # tt() # self._observation = self.render(mode='rgb') # reward = self._reward() # done = self._termination() # self.obs[:], rewards, self.dones, infos # return np.array(self._observation), reward, done, {} return the RBG image         :param mode: 'robot' to get the robot view         :return: # 145 degree # -36 degree