def attach_cube(
        self,
        simulation_program: SimulationProgram):
    """Adds an instruction to the given program, which grips the cube.
    Additionally executes the predefined `on_grip` callable, if given.

    Args:
        simulation_program: the simulation program to extend
    """
    assert not simulation_program.resulting_cube_state().cube_is_gripped, 'Cube already attached.'
    simulation_program.append_instruction(GripInstruction(True))

    if callable(self.on_grip):
        simulation_program.append_instruction(PyAnimationInstruction(
            name='on_grip',
            animation=self.on_grip
        ))

def detach_cube(
        self,
        simulation_program: SimulationProgram):
    """Adds an instruction to the given program, which releases the cube.

    Args:
        simulation_program: the simulation program to extend
    """
    assert simulation_program.resulting_cube_state().cube_is_gripped, 'Cube already detached.'
    simulation_program.append_instruction(GripInstruction(False))

    if callable(self.on_release):
        simulation_program.append_instruction(PyAnimationInstruction(
            name='on_release',
            animation=self.on_release
        ))

def enter_mount(
        self,
        target_factory: grip_target_generation.GripTargetFactory,
        simulation_program: SimulationProgram,
        attach_after_entering: bool = False,
        prepare_linear: bool = False,
        exit_mount_first_if_needed: bool = True) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which lets the robot enter the mount in a
    given manner described by the target factory and various flags.

    Args:
        target_factory: the grip target factory
        simulation_program: the parent simulation program
        attach_after_entering: flag, indicating whether to grip the cube after entering the mount
        prepare_linear: flag, indicating whether to move the robot arm linearly to the prepare position
        exit_mount_first_if_needed:  flag, indicating whether to exit the mount first if still inside (raises exception otherwise)

    Returns:
        the follow-up simulation program (child)
    """
    simulation_program = simulation_program.create_follow_up('enter_mount')

    if simulation_program.resulting_cube_state().cube_is_gripped and simulation_program.resulting_cube_state().cube_is_in_mount:
        if exit_mount_first_if_needed:
            simulation_program =  self.exit_mount(simulation_program)
        else:
            raise ValueError("Can't enter mount. Already there.")

    self.move(
        target_factory.get_grip_position(grip_target_generation.GripPositionType.get_type(
            target_factory.face_to_mount,
            simulation_program.resulting_cube_state().cube_is_gripped
        )),
        simulation_program,
        linear=prepare_linear
    )
    self.move(target_factory.get_grip_position(grip_target_generation.GripPositionType.GRIP), simulation_program, linear=True)
    if attach_after_entering:
        self.attach_cube(simulation_program)

    return simulation_program

def exit_mount(
        self,
        simulation_program: SimulationProgram,
        detach_before_exiting: bool = False) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which lets the robot exit the mount.

    Args:
        simulation_program: the parent simulation program
        detach_before_exiting:  flag, indicating whether to release the cube before exiting the mount

    Returns:
        the follow-up simulation program (child)
    """
    simulation_program = simulation_program.create_follow_up('exit_mount')

    cube_state = simulation_program.resulting_cube_state()
    if cube_state.cube_is_in_mount and cube_state.grip_position is not None:
        if detach_before_exiting and simulation_program.resulting_cube_state().cube_is_gripped:
            self.detach_cube(simulation_program)

        # to exit the mount we can simply reuse the factory we used to get to this position
        factory = cube_state.grip_position.target_factory
        self.move(
            factory.get_grip_position(grip_target_generation.GripPositionType.get_type(
                factory.face_to_mount,
                simulation_program.resulting_cube_state().cube_is_gripped
            )),
            simulation_program,
            linear=True
        )
    else:
        raise ValueError("Can't exit mount, if I haven't entered yet.")

    return simulation_program

def move(
        self,
        position: Union[grip_target_generation.GripPosition, robolink.Item],
        simulation_program: SimulationProgram,
        linear: bool = False):
    """Adds an instruction to the given program, which moves the robot to a specific position.

    Args:
        position: the grip position or robodk target to move to
        simulation_program: the simulation program to extend
        linear: flag, indicating whether to move the robot arm linearly
    """
    target = position
    if isinstance(position, grip_target_generation.GripPosition):
        target = position.target

    if not isinstance(target, robolink.Item) or not target.Valid():
        raise ValueError('Specified target is invalid.')

    simulation_program.append_instruction(MoveInstruction(
        target,
        grip_position=position if isinstance(position, grip_target_generation.GripPosition) else None,
        linear=linear
    ))

def move_to_idle(
        self,
        simulation_program: SimulationProgram) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which performs a movement to the idle position.

    Takes into account if the cube is currently gripped and mounted to ensure safe execution.

    Args:
        simulation_program: the parent simulation program

    Returns:
        the follow-up simulation program (child)
    """
    simulation_program = simulation_program.create_follow_up('move_to_idle')
    cube_state = simulation_program.resulting_cube_state()
    if cube_state.grip_position is not None and cube_state.cube_is_in_mount:
        factory = cube_state.grip_position.target_factory
        self.move(
            factory.get_grip_position(grip_target_generation.GripPositionType.get_type(
                factory.face_to_mount,
                simulation_program.resulting_cube_state().cube_is_gripped
            )),
            simulation_program,
            linear=True
        )

    # else: cube is not in mount or arm is not at grip -> we dont need to move to prepare position first
    self.move(RoboDKInterface.idle_target, simulation_program)
    return simulation_program

def perform_regrip(
        self,
        future_faces: List[cube.FacePosition],
        simulation_program: SimulationProgram) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which lets the robot perform a regrip.

    The algorithm therefore determines how to release and regrip the cube so
    the most future faces are accessible afterwards.

    Args:
        future_faces: the faces (in order) which should be accessible in the future
        simulation_program: the parent simulation program

    Returns:
        the follow-up simulation program (child)
    """
    time_start = time.time()
    possible_rotations = list(range(4))

    _, idx = np.unique(future_faces, return_index=True)
    desired_faces = np.array(future_faces)[np.sort(idx)][:3]
    desired_faces = list(map(lambda x: cube.FacePosition(x), desired_faces))

    if not simulation_program.resulting_cube_state().cube_is_gripped:
        return self.search_best_grip(
            desired_faces=desired_faces,
            simulation_program=simulation_program
        )[1][2]  # return the simulation program

    max_num_possible_future_moves = 0
    possible_programs: List[Tuple[List[cube.FacePosition], int, SimulationProgram]] = []

    # IMPROVEMENT: maybe put the next used face to the back?
    for mounted_face_on_release in grip_target_generation.GripperFace.ALL_ACCESSIBLE_GRIPPER_FACES:
        for release_rotation in possible_rotations:
            release_factory = grip_target_generation.GripTargetFactory(
                face_to_mount=mounted_face_on_release,
                num_rotations=release_rotation
            )

            try:
                release_program = simulation_program.create_follow_up('release')
                release_program = self.move_to_idle(release_program)
                release_program = self.enter_mount(release_factory, release_program)
                release_program = self.exit_mount(release_program, detach_before_exiting=True)
            except robolink.TargetReachError:
                continue

            try:
                max_num_possible_future_moves, possible_grip_program = self.search_best_grip(
                    desired_faces=desired_faces,
                    simulation_program=release_program,
                    release_factory=release_factory,
                    max_num_possible_future_moves=max_num_possible_future_moves
                )
                possible_programs.append(possible_grip_program)
            except ValueError as ex:
                logger.info('Could not find regrip for {} (n: {}). Exception: {}'.format(release_factory.face_to_mount, release_factory.num_rotations, ex))

    if len(possible_programs) < 1:
        raise ValueError('Couldnt find any non colliding regrip program.')

    possible_programs = list(filter(lambda program: program[1] == max_num_possible_future_moves, possible_programs))
    possible_programs.sort(key=lambda program: program[2].collision_checking_result.time)

    # IMPROVEMENT: prefer program, where the mounted face is the next face to rotate
    fastest_program_result = possible_programs[0]
    accessible_faces, num_possible_future_moves, simulation_program = fastest_program_result

    if logger.isEnabledFor(logging.INFO):
        logger.info('best program fits {}x -> {} (accessible faces: {}) -> {}'.format(
            num_possible_future_moves,
            list(map(lambda x: str(x), desired_faces[:num_possible_future_moves])),
            (' '.join(map(lambda x: '{} ({})'.format(x[0], str(x[1])), zip(['z+', 'y+', 'y-'], accessible_faces)))),
            str(simulation_program.collision_checking_result)
        ))
        time_end = time.time()
        logger.info('Analysis took me {:.0f}ms'.format((time_end - time_start) * 1000))

    assert desired_faces[0] in simulation_program.resulting_cube_state().exposed_faces, 'Accessible Faces should be {}\nbut actually are {}.'.format(
        (' '.join(map(lambda x: '{} ({})'.format(x[0], str(x[1])), zip(['z+', 'y+', 'y-'], accessible_faces)))),
        (' '.join(map(lambda x: '{} ({})'.format(x[0], str(x[1])), zip(['z+', 'y+', 'y-'], simulation_program.resulting_cube_state().exposed_faces))))
    )
    return simulation_program

def perform_rotations(
        self,
        rotations: List[spatial.Rotation],
        simulation_program: SimulationProgram) -> SimulationProgram:
    """Extends the given simulation program to perform a given set of rotations.

    Args:
        rotations: the rotations to perform on the cube
        simulation_program: the simulation program to extend

    Returns:
        the extended simulation program
    """

    num_rotations = len(rotations)
    faces = list(map(lambda rotation: rotation.face, rotations))

    for i, rotation in enumerate(rotations):
        assert isinstance(rotation, spatial.Rotation), 'expected: {}, got: {}'.format(spatial.Rotation, type(rotation))

        regrip_is_needed = not simulation_program.resulting_cube_state().cube_is_gripped
        if regrip_is_needed:
            logger.debug(f'Grabbing myself a sweet cubie to work with :-)')
        else:
            currently_exposed_faces = simulation_program.resulting_cube_state().exposed_faces
            regrip_is_needed = rotation.face not in currently_exposed_faces
            if regrip_is_needed:
                logger.debug('perform regrip to {} (currently accessible: {})'.format(
                    str(rotation.face), currently_exposed_faces
                ))

        if regrip_is_needed:
            try:
                simulation_program = self.perform_regrip(faces[i:], simulation_program)
            except robolink.TargetReachError:
                simulation_program = self.move_to_idle(simulation_program)
                simulation_program = self.perform_regrip(faces[i:], simulation_program)

        logger.debug('{} of {}: perform {} rotation -> {}x({})'.format(i+1, num_rotations, rotation, rotation.n, str(rotation.face)))
        try:
            simulation_program = self.rotate_face(rotation.face, rotation.n, simulation_program, regrip_if_needed=False)
        except robolink.TargetReachError:
            simulation_program = self.move_to_idle(simulation_program)
            simulation_program = self.rotate_face(rotation.face, rotation.n, simulation_program, regrip_if_needed=False)

        logger.debug('rotation {} done\n\n'.format(rotation))

    return simulation_program

def rotate(
        self,
        num_rotations: int,
        simulation_program: SimulationProgram) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which lets the robot rotate the mounted face.

    Args:
        num_rotations: the number of 90° turns the robot should perform
        simulation_program: the parent simulation program

    Returns:
        the follow-up simulation program (child)
    """

    simulation_program = simulation_program.create_follow_up('rotate')

    cube_state = simulation_program.resulting_cube_state()
    if not cube_state.cube_is_in_mount:
        raise ValueError('Cant rotate cube when not in mount.')
    current_factory = cube_state.grip_position.target_factory

    mounted_face = cube_state.exposed_face(current_factory.face_to_mount)
    if callable(self.on_rotation_start):
        simulation_program.append_instruction(PyAnimationInstruction(
            name='on_rotation_start',
            animation=self.on_rotation_start,
            mounted_face=mounted_face,
            num_rotations=num_rotations
        ))

    # IMPROVEMENT: maybe use 'überschleifen' here

    # rotate without leaving
    if abs(num_rotations) == 2:
        # avoid going around the back the side - so sometimes it has to be 1 and sometimes -1
        try:
            intermediate_factory = current_factory.rotated(-1)
            self.move(intermediate_factory.get_grip_position(grip_target_generation.GripPositionType.GRIP), simulation_program, linear=True)
        except robolink.TargetReachError:
            intermediate_factory = current_factory.rotated(1)
            self.move(intermediate_factory.get_grip_position(grip_target_generation.GripPositionType.GRIP), simulation_program, linear=True)

    destination_factory = current_factory.rotated(num_rotations)
    self.move(destination_factory.get_grip_position(grip_target_generation.GripPositionType.GRIP), simulation_program, linear=True)

    if callable(self.on_rotation_end):
        simulation_program.append_instruction(PyAnimationInstruction(
            name='on_rotation_end',
            animation=self.on_rotation_end,
            mounted_face=mounted_face,
            num_rotations=num_rotations
        ))

    return simulation_program

def rotate_face(
        self,
        face: cube.FacePosition,
        n: int,
        simulation_program: SimulationProgram,
        regrip_if_needed: bool = True) -> SimulationProgram:
    """Returns a follow-up of the given simulation program, which lets the robot rotate a given absolute face.

    The algorithm therefore determines how to hold the cube so the desired face is mounted.
    Also checks the `regrip_if_needed`-flag to perform a regrip if the current face is obstructed otherwise.

    Args:
        face: the face to rotate
        n: the number of rotations (90° turns) to perform
        simulation_program: the parent simulation program
        regrip_if_needed: flag, indicating whether to perform a regrip if otherwise not possible

    Returns:
        the follow-up simulation program (child)
    """

    # find face -> is it exposed? -> regrip if needed
    if face not in list(simulation_program.resulting_cube_state().exposed_faces):
        if regrip_if_needed:
            # perform regrip
            simulation_program = self.perform_regrip([face], simulation_program)
        else:
            raise ValueError('Face is not exposed yet. Regrip necessary.')

    # find best starting position
    start_time = time.time()
    gripper_face = grip_target_generation.GripperFace.ALL_ACCESSIBLE_GRIPPER_FACES[simulation_program.resulting_cube_state().exposed_faces.index(face)]
    logger.debug("Rotating {} ({})".format(str(face), str(gripper_face)))

    default_factory = grip_target_generation.GripTargetFactory(face_to_mount=gripper_face)
    programs = []
    for rotation_offset in range(4):
        factory = default_factory.rotated(rotation_offset)

        try:
            test_program: SimulationProgram = simulation_program.create_follow_up('rotate_face')
            test_program = self.enter_mount(factory, test_program)
            test_program = self.rotate(n, test_program)
        except robolink.TargetReachError:
            continue
        # NOTE: programs use actual current position of robot -> if we simulate, robot doenst move -> wrong starting pos here
        # -> this is why we need to use the 'flattened_standalone' version, which includes the movements of parent programs
        RoboDKProgram.perform_remaining_collision_checks(test_program, program_name_prefix='test_program')

        programs.append((factory, test_program))

    valid_programs = list(filter(lambda program: program[1].collision_checking_result.path_validity_percentage == 1, programs))
    valid_programs.sort(key=lambda program: program[1].collision_checking_result.time)

    if logger.isEnabledFor(logging.DEBUG):
        log_text = 'Num valid programs found: {}'.format(len(valid_programs))
        for (factory, sim_program) in programs:
            result = sim_program.collision_checking_result
            is_valid = result.path_validity_percentage == 1
            if not is_valid:
                log_text += '\033[31m'
            log_text += str(factory.num_rotations) + ' -> ' + str(result)
            if not is_valid:
                log_text += '\033[0m'

        end_time = time.time()
        log_text += 'Analysis took me {:.0f}ms'.format((end_time - start_time) * 1000)
        logger.debug(log_text)

    if len(valid_programs) < 1:
        raise ValueError('Couldnt find any non colliding rotation program.')

    fastest_program = valid_programs[0][1]
    return fastest_program

def search_best_grip(
        self,
        desired_faces: List[cube.FacePosition],
        simulation_program: SimulationProgram,
        release_factory: Optional[grip_target_generation.GripTargetFactory] = None,
        max_num_possible_future_moves: int = 0) -> Tuple[int, Tuple[List[cube.FacePosition], int, SimulationProgram]]:
    """Searches the best grip position to take for a given list of faces, which should be exposed afterwards
    and returns a follow-up program which performs the necessary actions (collision checked).

    Args:
        desired_faces: the faces which should be exposed
        simulation_program: the parent simulation program
        release_factory: a factory for the grip position in which the cube was released beforehand (only used for efficiency purposes)
        max_num_possible_future_moves: the number of faces in order which are accessible

    Returns:
        A tuple determining how many faces any solution could make available at most + a nested tuple describing
         the fastest program found: accessible faces, number of possible moves and the follow-up simulation program
    """

    time_start = time.time()
    possible_programs = []

    possible_mounted_faces = filter(lambda x: x not in desired_faces, grip_target_generation.GripperFace.ALL_ACCESSIBLE_GRIPPER_FACES)
    possible_rotations = list(range(0, 4))
    for mounted_face_on_regrip in possible_mounted_faces:
        for regrip_rotation in possible_rotations:
            if isinstance(release_factory, grip_target_generation.GripTargetFactory)\
                    and mounted_face_on_regrip == release_factory.face_to_mount\
                    and regrip_rotation == release_factory.num_rotations:
                # no changes dont do anything -> captain obvious strikes back
                continue

            regrip_factory = grip_target_generation.GripTargetFactory(
                face_to_mount=mounted_face_on_regrip,
                num_rotations=regrip_rotation
            )
            try:
                regrip_program = simulation_program.create_follow_up('regrip')
                regrip_program = self.enter_mount(regrip_factory, regrip_program, attach_after_entering=True)
            except robolink.TargetReachError:
                try:
                    regrip_program = simulation_program.create_follow_up('regrip')
                    regrip_program = self.move_to_idle(regrip_program)
                    regrip_program = self.enter_mount(regrip_factory, regrip_program, attach_after_entering=True)
                except robolink.TargetReachError:
                    continue

            accessible_faces = regrip_program.resulting_cube_state().exposed_faces

            num_possible_future_moves = 0
            for face in desired_faces:
                if face in accessible_faces:
                    num_possible_future_moves += 1
                else:
                    break

            if num_possible_future_moves >= max_num_possible_future_moves:
                # NOTE: programs use actual current position of robot -> if we simulate, robot doenst move -> wrong starting pos here
                # -> this is why we need to use the 'flattened_standalone' version, which includes the movements of parent programs
                RoboDKProgram.perform_remaining_collision_checks(regrip_program, 'test_regrip_program')

                if regrip_program.collision_checking_result.path_validity_percentage == 1:
                    if num_possible_future_moves > max_num_possible_future_moves:
                        max_num_possible_future_moves = num_possible_future_moves

                    possible_programs.append((
                        accessible_faces,
                        num_possible_future_moves,
                        regrip_program
                    ))

    if len(possible_programs) < 1:
        raise ValueError('Couldnt find any non colliding regrip program.')

    possible_programs = list(filter(lambda program: program[1] == max_num_possible_future_moves, possible_programs))
    possible_programs.sort(key=lambda program: program[2].collision_checking_result.time)

    fastest_program = possible_programs[0]
    accessible_faces, num_possible_future_moves, simulation_program = fastest_program
    if logger.isEnabledFor(logging.DEBUG):
        logger.debug('best grip fits {}x -> {} (accessible faces: {}) -> {}'.format(
            max_num_possible_future_moves,
            tuple(map(lambda x: str(x), desired_faces[:num_possible_future_moves])),
            (' '.join(map(lambda x: '{} ({})'.format(x[0], str(x[1])), zip(['z+', 'y+', 'y-'], accessible_faces)))),
            str(simulation_program.collision_checking_result)
        ))
        time_end = time.time()
        logger.debug('Analysis took me {:.0f}ms'.format((time_end - time_start) * 1000))

    # IMPROVEMENT: prefer program, where the mounted face is the next face to rotate
    return max_num_possible_future_moves, fastest_program

def on_grip(self):
    """
    This method is called when the cube is gripped (i.e. gripped from mount) in a SimulationProgram.
    It is for visualization purposes in RoboDK only.
    """
    RoboDKInterface.gripper.close()
    RoboDKInterface.ui_cube.ui_cube_frame.setParentStatic(RoboDKInterface.gripper.tool)

def on_release(self):
    """
    This method is called when the cube is released (i.e. put in mount) in a SimulationProgram.
    It is for visualization purposes in RoboDK only.
    """
    RoboDKInterface.gripper.open()
    RoboDKInterface.ui_cube.ui_cube_frame.setParentStatic(RoboDKInterface.grip_frame)

def on_rotation_end(self, mounted_face: cube.FacePosition, num_rotations: int):
    """
    This method is called after a face of the cube is rotated.
    It is for visualization purposes in RoboDK only.
    """
    RoboDKInterface.ui_cube.rotate(face=mounted_face, n=num_rotations)

def on_rotation_start(self, mounted_face: cube.FacePosition, num_rotations: int):
    """
    This method is called before a face of the cube is rotated.
    It is for visualization purposes in RoboDK only.
    """
    logger.debug('mounted_face: {} | rotations: {}'.format(str(mounted_face), num_rotations))
    RoboDKInterface.ui_cube.detach_face(mounted_face)