# Path: stripstream/utils.py
# def flatten(iterable_of_iterables):
#     return (item for iterables in iterable_of_iterables for item in iterables)
# 
# Path: robotics/openrave/transforms.py
# def pose_from_quat_point(quat, point):
#     return np.concatenate([quat, point])
# 
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def unit_quat():
#     return np.array((1, 0, 0, 0))


from inspect import currentframe, getfile
from itertools import product
from random import sample
from stripstream.utils import flatten
from utils import set_base_conf, Pose, box_body, get_name,  set_manipulator_conf, mirror_arm_config, open_gripper, close_gripper, top_grasps, Grasp,  SURFACE_Z_OFFSET, compute_surface
from robotics.openrave.transforms import pose_from_quat_point, set_pose, unit_quat
import os
import math
import numpy as np

PROBLEMS_DIR = os.path.dirname(os.path.abspath(getfile(currentframe())))
ENVIRONMENTS_DIR = os.path.join(PROBLEMS_DIR, 'environments')

BLACK = np.array((0, 0, 0, 0))
GREY = np.array((.5, .5, .5, 0))
WHITE = np.array((1, 1, 1, 0))

RED = np.array((1, 0, 0, 0))
GREEN = np.array((0, 1, 0, 0))
BLUE = np.array((0, 0, 1, 0))

YELLOW = np.array((1, 1, 0, 0))
PINK = np.array((1, 0, 1, 0))
CYAN = np.array((0, 1, 1, 0))

TAN = np.array((255, 165, 79, 0)) / 255.0


class RealProblem(object):

    def __init__(self, movable_names=[], surfaces=[], goal_surfaces={}):
        self.movable_names = movable_names
        self.surfaces = surfaces
        self.goal_surfaces = goal_surfaces

    def __str__(self):
        s = self.__class__.__name__ + '\n'
        for name, value in self.__dict__.iteritems():
            s += name + ': %s\n' % str(value)

# Path: stripstream/utils.py
# def flatten(iterable_of_iterables):
#     return (item for iterables in iterable_of_iterables for item in iterables)
# 
# Path: robotics/openrave/transforms.py
# def pose_from_quat_point(quat, point):
#     return np.concatenate([quat, point])
# 
# def set_pose(body, pose):
#     set_trans(body, trans_from_pose(pose))
# 
# def unit_quat():
#     return np.array((1, 0, 0, 0))


# Path: robotics/openrave/problems.py
from inspect import currentframe, getfile
from itertools import product
from random import sample
from stripstream.utils import flatten
from utils import set_base_conf, Pose, box_body, get_name,  set_manipulator_conf, mirror_arm_config, open_gripper, close_gripper, top_grasps, Grasp,  SURFACE_Z_OFFSET, compute_surface
from robotics.openrave.transforms import pose_from_quat_point, set_pose, unit_quat
import os
import math
import numpy as np


PROBLEMS_DIR = os.path.dirname(os.path.abspath(getfile(currentframe())))
ENVIRONMENTS_DIR = os.path.join(PROBLEMS_DIR, 'environments')

BLACK = np.array((0, 0, 0, 0))
GREY = np.array((.5, .5, .5, 0))
WHITE = np.array((1, 1, 1, 0))

RED = np.array((1, 0, 0, 0))
GREEN = np.array((0, 1, 0, 0))
BLUE = np.array((0, 0, 1, 0))

YELLOW = np.array((1, 1, 0, 0))
PINK = np.array((1, 0, 1, 0))
CYAN = np.array((0, 1, 1, 0))

TAN = np.array((255, 165, 79, 0)) / 255.0


class RealProblem(object):

    def __init__(self, movable_names=[], surfaces=[], goal_surfaces={}):
        self.movable_names = movable_names
        self.surfaces = surfaces
        self.goal_surfaces = goal_surfaces

    def __str__(self):
        s = self.__class__.__name__ + '\n'
        for name, value in self.__dict__.iteritems():
            s += name + ': %s\n' % str(value)

        return s