import numpy as np


def hold_timer(suc_bool, ep_timesteps, real_t_per_ts, min_time, start_time):
    """ Get result of a timer to decide whether a success reward criteria has been met for long enough """
    if ep_timesteps <= 1:
        start_time = None  # acts as a reset

    done_success = False
    if suc_bool:
        if start_time is None:
            start_time = ep_timesteps
        elif (ep_timesteps - start_time) * real_t_per_ts > min_time:
            done_success = True
    else:
        start_time = None

    return done_success, start_time


def get_world_ee_pose(env):
    return env.gripper.manipulator.get_link_pose(env.gripper.manipulator._tool_link_ind)


def get_world_obj_pose(env, obj_id):
    pos, quat = env._pb_client.getBasePositionAndOrientation(obj_id)
    return (*pos, *quat)