import os
import numpy as np
from argparse import Namespace

from manipulator_learning.sim.envs.color_blocks import ColorBlocks
from manipulator_learning.sim.utils import transformations as tf
from manipulator_learning.sim.utils.general import convert_quat_tf_to_pb


class ColorBlocksGenerator():
    def __init__(self, task, robot, collect_device='agent', left_hand=False, moving_base=False, egl=False,
                 poses_ref_frame='b', image_width=160, image_height=120, control_frame='t'):

        self.simple_config = dict(**locals())  # ugly, sorry for anybody who reads this
        del self.simple_config['self']  # ugh

        self.env_config = self.get_config(Namespace(**self.simple_config))

    def get_env(self):
        return ColorBlocks(**self.env_config)

    def get_config(self, args):

        jaco_robots = ['jaco_2_finger', 'jaco_3_finger']
        thing_robots = ['thing_2_finger', 'thing_rod', 'thing_panda_gripper', 'pretty_for_plots']

        # defaults
        block_style = 'cube'
        goal_type = None
        block_random_lim = []
        init_block_pos = []
        init_rod_pos = None
        rod_random_lim = None
        block_colors = None
        random_base_theta_bounds = (0, 0)
        robot_config = {}
        gripper_default_close = False
        base_pose_from_workspace_center = True  # TODO confirm how this works
        if 'fast_grip' in args.task and 'panda' not in args.robot:
            max_gripper_vel = 2.4
        else:
            max_gripper_vel = 0.8
        if args.poses_ref_frame is not None:
            poses_ref_frame = args.poses_ref_frame
        else:
            poses_ref_frame = 'b'
        workspace_center = (0.75, -.15, .7)
        gripper_force = 10
        valid_trans_dof = [1, 1, 1]
        valid_rot_dof = [1, 1, 1]

        if 'xy' in args.task and 'xyz' not in args.task:
            valid_trans_dof = [1, 1, 0]
            valid_rot_dof = [0, 0, 0]
        if 'xyz' in args.task:
            valid_rot_dof = [0, 0, 0]
        elif '6dof' in args.task:
            valid_rot_dof = [1, 1, 1]

        control_frame = args.control_frame
        init_gripper_random_lim = None

        new_tasks_list = ['lift_xyz', 'bring_xyz', 'stack_2_same_xyz', 'door']
        # start of older, less generic, messier task definitions
        if args.task == 'grasp_and_place' or 'pick_and_place' in args.task:
            # args.task == 'pick_and_place_xyz' or \
            #     'pick_and_place_6dof' in args.task:
            if args.robot in jaco_robots:
                block_random_lim = [[.1, .25]]
                init_block_pos = [[-.15, 0]]
                random_base_theta_bounds = (-np.pi / 16, 3 * np.pi / 16)
            elif args.robot in thing_robots:
                block_random_lim = [[.25, .1]]
                if 'small_box' in args.task:
                    block_random_lim = [[.05, .05]]
                init_block_pos = [[.05, -0.05]]
                random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
                # todo figure out why the noises here NOT being set to 0 break the simulation  ''\_'_'_/''
                base_random_lim = ((.02, .02, .002), (0, 0, .02))
            elif args.robot == 'panda':
                block_random_lim = [[.25, .1]]
                if 'small_box' in args.task:
                    block_random_lim = [[.05, .05]]
                init_block_pos = [[.05, -0.05]]
                random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
                base_random_lim = ((.02, .02, .002), (0, 0, .02))
            if 'air' in args.task:
                goal_type = 'air'
            else:
                goal_type = 'plate'
            if args.task == 'grasp_and_place':
                block_style = 'cube'
            elif 'xyz' in args.task or '6dof' in args.task:
                init_block_pos = [[0.0, -0.05]]
                valid_trans_dof = [1, 1, 1]
                if 'xyz' in args.task:
                    valid_rot_dof = [0, 0, 0]
                    block_style = 'cube'
                    control_frame = 'w'
                elif '6dof' in args.task:
                    valid_rot_dof = [1, 1, 1]
                    block_style = 'cube'
                    control_frame = 'w'
                if 'pick_and_place_6dof_long' in args.task or 'small_box' in args.task:
                    block_style = 'long'
            elif args.task == 'pick_and_place_xy' or args.task == 'pick_and_place_xy_vert':
                if args.task == 'pick_and_place_xy':
                    block_random_lim = [[.25, .1]]
                    init_block_pos = [[.05, 0.15]]
                elif args.task == 'pick_and_place_xy_vert':
                    block_random_lim = [[.1, .25]]
                    init_block_pos = [[.15, .05]]
                random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
                block_style = 'cylinder'
                goal_type = 'plate'
                control_frame = 'w'
                valid_trans_dof = [1, 1, 0]
                valid_rot_dof = [0, 0, 0]

        elif 'sort' in args.task:
            if args.task == 'sort_2':
                init_block_pos = [[0.0, -0.05], [0.0, -.05]]
                block_random_lim = [[.1, .1], [.1, .1]]
            elif args.task == 'sort_3':
                init_block_pos = [[0.0, -0.05], [0.0, -.05], [0.0, -.05]]
                block_random_lim = [[.05, .05], [.05, .05], [.05, .05]]
                block_colors = ['blue', 'green', 'blue']
            goal_type = 'plate'

        elif 'reaching' in args.task or args.task == 'reaching_xy':
            block_style = ''
            if args.robot in jaco_robots:
                block_random_lim = [[.2, .2]]
                init_block_pos = [[-.05, 0]]
                random_base_theta_bounds = (-np.pi / 16, 3 * np.pi / 16)
            elif args.robot in thing_robots:
                # block_random_lim = [[.2, .2]]  # all old data is with this!!!
                block_random_lim = [[.35, .35]]
                init_block_pos = [[.1, 0]]
                random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
            elif args.robot == 'panda':
                block_random_lim = [[.35, .35]]
                workspace_center = (1.0, -.2, .7)
                init_block_pos = [[-.025, -.05]]
                block_style = 'small'
            goal_type = None
            # block_style = 'tall'
            if args.task == 'reaching_xy':
                block_style='vis_only'
                control_frame = 'w'
                valid_trans_dof = [1, 1, 0]
                valid_rot_dof = [0, 0, 0]
        elif args.task =='pushing_easy':
            if args.robot in jaco_robots:
                block_random_lim = [[.1, .25], [0, 0]]
                init_block_pos = [[-.15, 0], [.05, 0]]
            elif args.robot in thing_robots:
                block_random_lim = [[.25, .1], [0, 0]]
                init_block_pos = [[-.05, -.2], [-.05, 0]]
            random_base_theta_bounds = (-np.pi / 8, np.pi / 8)
            goal_type = None
            block_style = 'low_fric'
        elif 'pushing' in args.task:
            if args.robot in jaco_robots:
                block_random_lim = [[.1, .25], [0, 0]]
                init_block_pos = [[-.15, 0], [.05, 0]]
                goal_type = None
            elif args.robot in thing_robots:
                if 'coaster' in args.task:
                    # block_random_lim = [[.25, .1]]
                    block_random_lim = [[.1, .1]]
                    init_block_pos = [[0, -.05]]
                    goal_type = 'coaster'
                else:
                    block_random_lim = [[.25, .1], [0, 0]]
                    init_block_pos = [[0, 0], [0, .2]]
                    goal_type = None
            block_style = 'low_fric'
            gripper_default_close = True
            random_base_theta_bounds = (-np.pi / 8, np.pi / 8)
            if args.task == 'pushing_xy':
                control_frame = 'w'
                valid_trans_dof = [1, 1, 0]
                valid_rot_dof = [0, 0, 0]
        elif 'stack' in args.task:
            # block_random_lim = [[.25, .25], [.25, .25], [.25, .25]]
            # init_block_pos = [[0, 0], [0, 0], [0, 0]]
            # block_random_lim = [[.1, .1], [.1, .1]]
            # block_random_lim = [[.1, .1], [.1, .1]]
            if args.task == 'stack_2':
                block_random_lim = [[.1, .1], [.1, .1]]
                init_block_pos = [[-.05, 0], [0, .15]]
            elif args.task == 'stack_2_small' or args.task == 'stack_2_narrow_small':
                block_random_lim = [[.05, .05], [.05, .05]]
                init_block_pos = [[-.05, 0], [0, .15]]
            elif '2_same' in args.task:
                block_random_lim = [[.15, .15], [.15, .15]]
                init_block_pos = [[0., 0.], [0., 0.]]
            elif args.task == 'stack_3':
                block_random_lim = [[.05, .05], [.05, .05], [.05, .05]]
                init_block_pos = [[0.0, .1], [.1, 0], [-.1, 0]]
            goal_type = None
            if 'narrow' in args.task:
                block_style = 'tall_narrow'
            else:
                block_style = 'cube'
            gripper_default_close = False
            random_base_theta_bounds = (-np.pi / 8, np.pi / 8)
        elif args.task == 'insertion':
            gripper_default_close = True
            robot_config['gripper_max'] = [1, .2]
            random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
        elif 'pick_insertion' in args.task:
            gripper_default_close = False
            random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
            robot_config['gripper_max'] = [1, .2]
            init_rod_pos = [-.025, -.1]
            if 'small' in args.task:
                rod_random_lim = [.025, .025]
            else:
                rod_random_lim = [.1, .1]
            # rod_random_lim = [0., 0.]
        elif args.task in new_tasks_list:
            pass
        else:
            raise NotImplementedError("%s is not a defined task!" % args.task)



        # initial gripper rotation
        # init_gripper_rot = [np.pi, 3*np.pi/8, np.pi]
        init_gripper_rot = [np.pi, 7*np.pi/16, np.pi/2]
        init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='rxyz'))

        # select robot
        cur_path = os.path.dirname(os.path.abspath(__file__))
        robot_urdf_base = cur_path + '/../../robots'
        robot_config['robot'] = args.robot
        robot_config['arm_indices'] = []
        robot_config['base_constraint'] = False
        debug_cam_params = (.12, -.38, .78, 1.4, -27.8, 107.6)

        if args.task == 'pick_and_place_xy':
            goal_pos = [.05, 0.0]
        elif args.task == 'pick_and_place_xy_vert':
            goal_pos = [-.05, 0.0]
        elif args.task == 'pick_and_place_xyz':
            goal_pos = [0.0, .15]
        elif args.task == 'sort_2' or args.task == 'sort_3':
            goal_pos = [.2, .15]
        else:
            goal_pos = [.05, .15]

        if 'thing_panda_gripper' in args.robot:
            if args.robot == 'thing_panda_gripper':
                robot_config['urdf_root'] = robot_urdf_base + \
                                            '/models/thing/assets/combined_urdf/ridgeback_ur10_pandagrip.urdf'
            elif args.robot == 'thing_panda_gripper_fake_cam_mount':
                robot_config['urdf_root'] = robot_urdf_base + \
                                            '/models/thing/assets/combined_urdf/ridgeback_ur10_pandagrip_fake_cam_mount.urdf'
            elif args.robot == 'thing_panda_gripper_no_collision':
                robot_config['urdf_root'] = robot_urdf_base + \
                                            '/models/thing/assets/combined_urdf/ridgeback_ur10_pandagrip_no_collision.urdf'
            robot_config['num_controllable_joints'] = 8
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 1
            robot_config['ee_link_index'] = 17
            robot_config['tool_link_index'] = 21
            robot_config['gripper_indices'] = [19, 20]
            robot_config['arm_indices'] = [11, 12, 13, 14, 15, 16]
            robot_config['gripper_max'] = [.04, 0]

            robot_base_ws_cam_tf = ((-.4, .65, .9), (-2.45, 0, -.4))
            init_gripper_rot = [-.75 * np.pi, 0, np.pi / 2]
            init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
            init_gripper_pose = [[-.15, .85, 0.75], init_gripper_rot]
            robot_base_pose = [[0, -.5, 0], [0, 0, -1.5707963]]  # IGNORED if using moving base
            robot_config['base_constraint'] = True
            random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
        elif args.robot == 'panda':
            robot_config['urdf_root'] = robot_urdf_base + "/models/franka_panda/panda.urdf"
            robot_config['num_controllable_joints'] = 9
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 0
            robot_config['ee_link_index'] = 9
            robot_config['tool_link_index'] = 12
            robot_config['gripper_indices'] = [10, 11]
            robot_config['arm_indices'] = [1, 2, 3, 4, 5, 6, 7]
            robot_config['gripper_max'] = [.04, 0]

            robot_base_ws_cam_tf = ((-.4, .6, .3), (-2, 0, -1.85))
            init_gripper_rot = [np.pi, 0, 0]
            init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
            init_gripper_pose = [[0.0, .5, .25], init_gripper_rot]
            # robot_base_pose = [[1.0, -.7, .6247], [0, 0, 0]]  # IGNORED if using moving base
            robot_base_pose = [[1.0, -.7, .52], [0, 0, 0]]  # IGNORED if using moving base
            robot_config['base_constraint'] = True
            base_pose_from_workspace_center = False
            max_gripper_vel = 0.8
            workspace_center = (1.0, -.2, .7)
        elif args.robot == 'thing_2f85':
            robot_config['urdf_root'] = robot_urdf_base + \
                                        '/models/thing/assets/combined_urdf/ridgeback_ur10_2f85.urdf'
            robot_config['num_controllable_joints'] = 8
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 1
            robot_config['ee_link_index'] = 17
            robot_config['tool_link_index'] = 29
            robot_config['gripper_indices'] = [19, 24]
            robot_config['arm_indices'] = [11, 12, 13, 14, 15, 16]
            robot_config['gripper_max'] = [1, 0]
            robot_base_ws_cam_tf = ((-.4, .65, .9), (-2.45, 0, -.4))
            init_gripper_rot = [-.75 * np.pi, 0, np.pi / 2]
            init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
            init_gripper_pose = [[-.15, .85, 0.75], init_gripper_rot]
            robot_base_pose = [[0, -.5, 0], [0, 0, -1.5707963]]  # IGNORED if using moving base
            robot_config['base_constraint'] = True
        elif args.robot == 'thing_2_finger':
            robot_config['urdf_root'] = robot_urdf_base + \
                                        '/models/thing/assets/combined_urdf/ridgeback_ur10_pr2grip_fixed.urdf'
            robot_config['num_controllable_joints'] = 8
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 1
            robot_config['ee_link_index'] = 17
            robot_config['tool_link_index'] = 22
            robot_config['gripper_indices'] = [18, 20]
            robot_config['arm_indices'] = [11, 12, 13, 14, 15, 16]
            robot_config['gripper_max'] = [1, 0]
            if args.left_hand:
                robot_base_ws_cam_tf = ((-.3, .7, .95), (-.75 * np.pi, 0, 0))  # lower angle cam
                init_gripper_rot = [-.75 * np.pi, 0, np.pi / 2]
                init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
                init_gripper_pose = [[-.15, -.83, 0.95], init_gripper_rot]
                robot_base_pose = [[0, -.2, 0], [0, 0, 1.5707963]]
            else:
                # robot_base_ws_cam_tf = ((-.3, .8, 1.05), (3.44, 0, 0))
                # robot_base_ws_cam_tf = ((-.3, .7, .95), (-.75 * np.pi, 0, 0))  # lower angle cam
                # robot_base_ws_cam_tf = ((-.4, .65, 1), (-2.45, 0, -.4))  # different angle cam
                robot_base_ws_cam_tf = ((-.4, .65, .9), (-2.45, 0, -.4))  # different angle cam, fixed so base is above ground
                init_gripper_rot = [-.75 * np.pi, 0, np.pi/2]
                init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
                init_gripper_pose = [[-.15, .85, 0.75], init_gripper_rot]
                if args.task == 'reaching_xy' or args.task == 'pick_and_place_xy' or \
                    args.task == 'pick_and_place_xy_vert':
                    init_gripper_rot = [-.65 * np.pi, 0, np.pi / 2]
                    init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
                    init_gripper_pose = [[-.15, .85, 0.65], init_gripper_rot]
                if args.task == 'pick_and_place_xy' or args.task == 'pick_and_place_x_vert':
                    init_gripper_pose = [[-.15, .85, 0.64], init_gripper_rot]
                # if args.task == 'pick_and_place_xy':  # double table on hold
                #     init_gripper_pose = [[-.15, .85, 0.6], init_gripper_rot]
                robot_base_pose=[[0, -.5, 0], [0, 0, -1.5707963]]  # IGNORED if using moving base
            robot_config['base_constraint'] = True
        elif args.robot == 'thing_rod':
            robot_config['urdf_root'] = robot_urdf_base +  '/models/thing/' \
                                                   'assets/combined_urdf/ridgeback_ur10_rod_fixed.urdf'
            robot_config['num_controllable_joints'] = 6
            robot_config['num_gripper_joints'] = 0
            robot_config['base_link_index'] = 1
            robot_config['ee_link_index'] = 16
            robot_config['tool_link_index'] = 18

            robot_config['gripper_indices'] = []
            robot_config['arm_indices'] = [11, 12, 13, 14, 15, 16]
            robot_config['gripper_max'] = [1, 0]
            # robot_base_ws_cam_tf = ((-.3, .8, 1.05), (3.44, 0, 0))
            robot_base_ws_cam_tf = ((-.4, .65, 1), (-2.45, 0, -.4))  # different angle cam
            init_gripper_rot = [-.75 * np.pi, 0, np.pi/2]
            init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
            if args.task == 'pushing_xy':
                init_gripper_pose = [[-.15, .85, 0.72], init_gripper_rot]
            else:
                init_gripper_pose=[[-.15, .85, 0.75], init_gripper_rot]
            init_base_rot = [0, 0, -1.57]
            init_base_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_base_rot, axes='rxyz'))
            robot_base_pose=[[0, -.5, 0], [0, 0, -1.5707963]]
            robot_config['base_constraint'] = True
            goal_pos = [.05, .185]
        elif args.robot == 'jaco_2_finger':
            robot_config['urdf_root'] = cur_path + '/../../robots/pyb_manipulator/' \
                                                   'models/urdf/j2n6s300_2_finger_fixed_visuals.urdf'  #todo these are missing now
            robot_config['num_controllable_joints'] = 8
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 0
            robot_config['ee_link_index'] = 7
            robot_config['tool_link_index'] = 8
            robot_config['gripper_indices'] = [9, 11]
            robot_config['gripper_max'] = [0.3, 1.35]
            robot_base_ws_cam_tf = ((.55, -.25, .55), (160 / 180 * np.pi, 0, np.pi))
            init_gripper_pose = [[.4, -.35, .35], init_gripper_rot]
            robot_base_pose = ([.3, .2, .4], [0, 0, 0])
            goal_pos = [.05, 0]
            block_pos_modifier = [0, 0]
        elif args.robot == 'jaco_3_finger':
            robot_config['urdf_root'] = cur_path + \
                            '/../../robots/pyb_manipulator/models/urdf/jaco_fixed_visuals.urdf'
            robot_config['num_controllable_joints'] = 9
            robot_config['num_gripper_joints'] = 3
            robot_config['base_link_index'] = 0
            robot_config['ee_link_index'] = 7
            robot_config['tool_link_index'] = 8
            robot_config['gripper_indices'] = [9, 11, 13]
            robot_config['gripper_max'] = [0.3, 1.35]
            robot_base_ws_cam_tf = ((.55, -.25, .55), (160 / 180 * np.pi, 0, np.pi))
            init_gripper_pose = [[.4, -.35, .35], init_gripper_rot]
            robot_base_pose = ([.3, .2, .4], [0, 0, 0])
            goal_pos = [.05, 0]
            block_pos_modifier = [0, 0]
        elif args.robot == 'pretty_for_plots':
            robot_config['urdf_root'] = robot_urdf_base + '/models/thing/' \
                                                   'assets/combined_urdf/ridgeback_ur10_pr2grip_fixed.urdf'
            robot_config['num_controllable_joints'] = 8
            robot_config['num_gripper_joints'] = 2
            robot_config['base_link_index'] = 1
            robot_config['ee_link_index'] = 17
            robot_config['tool_link_index'] = 22
            robot_config['gripper_indices'] = [18, 20]
            robot_config['arm_indices'] = [11, 12, 13, 14, 15, 16]
            robot_config['gripper_max'] = [1, 0]
            robot_base_ws_cam_tf = ((-.4, .65, .9), (-2.45, 0, -.4))  # different angle cam, fixed so base is above ground
            init_gripper_rot = [-.75 * np.pi, 0, np.pi / 2]
            init_gripper_rot = convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))
            init_gripper_pose = [[-.15, .85, 0.65], init_gripper_rot]
            robot_base_pose = [[0, -.5, 0], [0, 0, -1.5707963]]  # IGNORED if using moving base
            robot_config['base_constraint'] = True
            goal_pos = [.05, .15]
            debug_cam_params = [.07, -.27, .79, 1.2, -87.4, 270]
        else:
            raise NotImplementedError("%s selection for robot is not implemented.", args.robot)

        # load the pb_cam_viewer

        # temp for paper figure
        debug_cam_params = [.20, -.41, .59, 1.6, -29.4, 156.]

        # start of newer, better, more generic task definitions
        if 'xyz' in args.task:
            valid_trans_dof = [1, 1, 1]
            valid_rot_dof = [0, 0, 0]
        if 'lift' in args.task or 'bring' in args.task:

            block_random_lim = [[0.25, 0.25]]
            # block_random_lim = [[0.0, 0.0]]

            if args.robot == 'thing_2_finger' or args.robot == 'thing_2f85':
                block_style = ''
                init_gripper_rot = [-.75 * np.pi, 0, .75 * np.pi]
                # roughly center xy, corresponds to 0,0 of initial random block pose
                init_gripper_pos = [-.29, .64, 0.75]
                workspace_center = (.75, -.15, .7)
                init_block_pos = [[0.0, 0.0]]
                robot_base_ws_cam_tf = ((-.4, .5, 1.0), (-2.55, 0, -.4))
                random_base_theta_bounds = (-3 * np.pi / 16, np.pi / 16)
            elif args.robot == 'panda':
                block_style = 'small'
                init_gripper_rot = [np.pi, 0, 0]
                init_gripper_pos = [0.0, .5, .25]
                workspace_center = (1.0, -.2, .7)
                init_block_pos = [[-.025, -.05]]

            init_gripper_random_lim = [.25, .25, .06, 0., 0., 0.]
            # init_gripper_random_lim = [0, 0, 0, 0., 0., 0.]
            init_gripper_pose = [init_gripper_pos,
                                 convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))]

            if 'lift' in args.task:
                goal_type = None
            elif 'bring' in args.task:
                goal_type = 'coaster'
                goal_pos = [0.0, .1]

        elif 'door' in args.task:
            gripper_force = 500
            block_random_lim = []
            init_block_pos = []
            workspace_center = (0.75, -.15, .7)
            init_gripper_random_lim = [.12, .05, .05, 0., 0., 0.]
            if 'thing_panda_gripper' in args.robot:
                # init_gripper_pos = [-.19, .74, 0.75]
                init_gripper_pos = [-.29, .7, 0.75]
                # init_gripper_rot = [-.75 * np.pi, 0, .75 * np.pi]
                init_gripper_rot = [-.75 * np.pi, 0, .125 * np.pi]
                init_gripper_pose = [init_gripper_pos,
                                 convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))]

        elif 'stack_2_same' in args.task and args.robot == 'panda':
            block_style = 'small'
            init_gripper_rot = [np.pi, 0, 0]
            init_gripper_pos = [0.0, .5, .25]
            # workspace_center = (1.0, -.2, .7)
            block_random_lim = [[0.15, 0.15], [0.15, 0.15]]
            init_block_pos = [[-.025, -.05], [-.025, -.05]]
            init_gripper_random_lim = [.15, .15, .06, 0., 0., 0.]
            # init_gripper_random_lim = [0, 0, 0, 0., 0., 0.]
            init_gripper_pose = [init_gripper_pos,
                                 convert_quat_tf_to_pb(tf.quaternion_from_euler(*init_gripper_rot, axes='sxyz'))]
        
        if args.moving_base:
            # base_random_lim = ((.02, .02, .002), (.02, .02, .02))
            base_random_lim = ((.02, .02, .002), (0, 0, .02))
        else:
            base_random_lim = ((0, 0, 0), (0, 0, 0))
            random_base_theta_bounds = (0, 0)

        env_config = dict(
            robot_config=robot_config,
            object_urdf_root = cur_path + '/../../objects/models/urdf',
            max_trans_acc = .015,  # UNUSED
            max_trans_vel=.25,    # UNUSED
            max_rot_vel=1.5,       # UNUSED
            control_mode=args.collect_device,  # always set to agent for now, but should be entirely removed
            randomize_blocks=True,  # UNUSED, random limits can just be set to 0, 0
            time_step=.01,
            t_control_style='v',  # UNUSED i think, but need to confirm
            r_control_style='v',  # UNUSED i think, but need to confirm
            block_style=block_style,  # check color blocks for options
            block_random_lim=block_random_lim,
            init_block_pos=init_block_pos,
            mark_on_table=False,   # should be removed, loading actual objects with no collision as alternative now
            goal_type=goal_type,
            goal_pos=goal_pos,
            ws_cam_eye=[.85, -.05, 1.05],  # UNUSUED with fake mobile base, should be removed
            ws_cam_target=[.85, -.3, .2],  # UNUSUED with fake mobile base, should be removed
            robot_base_ws_cam_tf=robot_base_ws_cam_tf,
            init_gripper_pose=init_gripper_pose,
            robot='jaco',  # TODO fix this to be part of robot config
            robot_base_pose=robot_base_pose,
            base_random_lim=base_random_lim,
            fake_mobile_base=True,
            pose_3_pts_dist=0.1,  # distance of each of the three pts for pose representation from the tool frame origin
            poses_ref_frame=poses_ref_frame,  # reference frame that pose observations are given in, b for base or w for world
            vel_ref_frame='a',  # reference frame that velocity observations are given in, t for tool, b for base,
                                # or 'a' for all  # TODO make this less confusing
            control_frame=control_frame,  # ref frame the velocity commands are given in, if controlled by velocity
            collect_lpf_tau=0.1,  # low pass filter for smooth trajs in collection, [0, 1], lower is more filtered
                                    # UNUSUED, and lots of crufty code around it in manipulator_env_wrapper to delete
            valid_rot_dof=valid_rot_dof,  # valid rotational degrees of freedom in given control frame
                                      # (in end effector frame (z out, x to right, y down)
            valid_trans_dof=valid_trans_dof,  # valid translational dof in given control frame
            render=not args.egl,
            renderer='opengl',  # either tiny or opengl, tiny is slower and egl only has opengl
            use_egl=args.egl,  # needs to be false for VR  # TODO remove having both render and use egl
            render_shadows=True,  # needs to be false for VR
            render_ground_plane=True,  # best idea is to be true for VR, to make egl rendering look the same
            display_on_headset=True,  # currently does nothing
            task=args.task,
            gripper_default_close=gripper_default_close,
            # configuration for setting the base pose based on maintaining a particular workspace view
            base_pose_from_workspace_center=base_pose_from_workspace_center,  # if this is true, base pose is set based on main block pos,
                                                               # and robot_bae_pose is ignored
            workspace_center=workspace_center,  # the center point of the workspace
            cam_workspace_distance=.3,  # distance from cam frame to workspace center
            random_base_theta_bounds=random_base_theta_bounds,  # min and max values of yaw of base while maintaining same
                                                                # cam workspace center point
            # cur_pose_control_noise=((0.005, 0.005, 0.005), (0.05, 0.05, 0.05)),  # uniform noise to add to output
                                                                     # task space control signals, non-additive
            cur_pose_control_noise=None,
            demo_random_walk_control_noise=None,  # uniform noise to add to output
                                                                     # task space control signals
            demo_drift_control_noise=((0.002, 0.002, 0.002), (0.001, 0.001, 0.001)),
            control_noise_freq=100,  # frequency of control noise, in Hz (related to real time, not sim time)
            debug_cam_params=debug_cam_params,  # default position for debug camera if gui is open
            image_height=args.image_height,
            image_width=args.image_width,
            init_rod_pos=init_rod_pos,
            rod_random_lim=rod_random_lim,
            block_colors=block_colors,
            init_gripper_random_lim=init_gripper_random_lim,
            max_gripper_vel=max_gripper_vel,
            gripper_force=gripper_force
        )

        # todo all initial poses should be in euler angles, not quaternions. Especially for randomizing initial base and gripper pose this will be neessary

        return env_config