def get_required_bounce_dir(self, backward_source_edge, backward_target_edge):

        p1, p2 = self.vertices_of_edges[backward_source_edge]
        p3, p4 = self.vertices_of_edges[backward_target_edge]
        dir_p1p3 = cal_angle(p1, p3)
        dir_p1p4 = cal_angle(p1, p4)
        dir_p2p3 = cal_angle(p2, p3)
        dir_p2p4 = cal_angle(p2, p4)
        bouncing_dir_p1p3 = self.updated_bouncing_direction(dir_p1p3, self.vertices_of_edges[backward_target_edge])
        bouncing_dir_p1p4 = self.updated_bouncing_direction(dir_p1p4, self.vertices_of_edges[backward_target_edge])            
        bouncing_dir_p2p3 = self.updated_bouncing_direction(dir_p2p3, self.vertices_of_edges[backward_target_edge])
        bouncing_dir_p2p4 = self.updated_bouncing_direction(dir_p2p4, self.vertices_of_edges[backward_target_edge])
        return
    def compute_initial_direction_exact_position(self, man_pos, target_edge):
        p1, p2 = target_edge
        angle_1 = cal_angle(man_pos, p1)
        angle_2 = cal_angle(man_pos, p2)

        if angle_1 > angle_2:
            if angle_1 - angle_2 > pi:
                return (angle_1 - pi_2, angle_2)
                #return [(0, angle_2), (angle_1, pi)]
            else:
                return (angle_2, angle_1)
        elif angle_2 - angle_1 >pi:
            return (angle_2 - pi_2, angle_1)
        else:
            return (angle_1, angle_2)
    def can_bounce(self, pos, surface):

        required_direction_1, required_direction_2 = self.get_required_direction_from_other_edges(surface)
        p1, p2 = self.vertices_of_edges[surface]

        angle1 = cal_angle(pos, p1)
        angle2 = cal_angle(pos, p2)
        if angle1 > angle2:
            if angle1 - angle2 > pi:
                direction_range = (angle1 - pi_2, angle2)
            else:
                direction_range = (angle2, angle1)
        else:
            if angle2 - angle1 > pi:
                direction_range = (angle2 - pi_2, angle1)
            else:
                direction_range = (angle1, angle2)

        if self.update_direction(direction_range, required_direction_1) != None:
            return True
        if self.update_direction(direction_range, required_direction_2) != None:
            return True
        return False
    def reflection_angle(self, direction1, surface_vec):
        def is_intersection(hit_dir, surface_dir):
            if hit_dir > surface_dir:
                difference = hit_dir - surface_dir
            else:
                difference = surface_dir - hit_dir
            _flag = False
            if difference > pi:
                difference = pi + pi - difference
                _flag = True

            if difference <= pi/2:
                result = surface_dir - hit_dir
                #print "pre plus: ", result
                if result < 0 and _flag:
                    result = pi + pi + result
                #print "result: ", result   
                return result
            else:
                return -1
    
        surface_angle1 = cal_angle(*surface_vec)
        
        if surface_angle1 > pi:
            surface_angle2 = surface_angle1 - pi
        else:
            surface_angle2 = surface_angle1 + pi

        if direction1 < 0:
            direction1 = direction1 + pi + pi

        difference = is_intersection(direction1, surface_angle1)
        if difference != -1:
            reflected_angle = surface_angle1 + difference
        else:
            #print is_intersection(direction1, surface_angle2)
            reflected_angle = surface_angle2 + is_intersection(direction1, surface_angle2)

        if reflected_angle >= pi + pi:
            reflected_angle = reflected_angle - pi - pi
        elif reflected_angle < 0:
            reflected_angle = reflected_angle + pi + pi
        return  reflected_angle
    def infer_next_states(self, current_state, pre_node):

        next_states = []
        #print "test: ", current_state
        current_node, entering_edge, current_direction, current_motion, is_collided, give_impulse_direction, obj_id = current_state
        #if current_node == 5 and entering_edge == frozenset([10,11]) and current_motion == SLIDING and not is_collided:
        #   print " " ," ", current_state
        #direction_x, direction_y = current_direction
        #print "current_node: ", current_node
        edges = self.edges_by_tri[current_node]

        #if current_node == 40:
        #    print "cdir before apply gravity: ", current_direction
        if current_motion == FLYING:
            ######### Add Gravity Here ##############################
            current_direction = self.apply_gravity(current_direction)
            ######### Add Gravity Here ##############################
            #if current_node == 5 and entering_edge == frozenset([10, 11]):
            #   print "after apply gravity: ", current_direction
            for edge in edges:

                #print self.is_edge_surface[edge]
                if edge != entering_edge and self.is_edge_surface[edge] != -1:
                    required_direction = self.get_required_direction(entering_edge, edge)
                    updated_direction = self.update_direction(current_direction, required_direction)
                    #if current_node == 5 and entering_edge == frozenset([10,11]):
                    #    print current_node, " cdir: ", current_direction, "rdir: ", required_direction,"  udir: ", updated_direction, " entering edge: ", entering_edge, " arrive edge", edge
                    direction_change_flag = False
                    
                    ############## ADD CODE FOR GRAVITY #######################
                    

                    ## consider gravity:
                    '''
                    if len(updated_direction)==0:                        
                        #merge_direction = self.merge_with_graivty_dir(current_direction)
                        #updated_direction = self.update_direction(merge_direction, required_direction)
                        updated_direction = self.update_direction([(GRAVITY_DIR,GRAVITY_DIR)], required_direction)
                        if len(updated_direction) != 0:
                            #print "use gravity at state: ", current_state, " rdir ", required_direction, " merge_dir: ", merge_direction 
                            direction_change_flag = True
                    '''
                    ############## ADD CODE FOR GRAVITY #######################

                    if updated_direction == None:
                    

                       #print " entering_edge: ", entering_edge, " current_direction: ", current_direction
                       continue 
                    else:  
                        if self.is_edge_surface[edge] == 1:
                            # does not allow moveable object id to bounce [unless it is circle]
                            if obj_id != self.initial_obj_id:
                                state = (current_node, edge, updated_direction, FLYING, False, updated_direction, obj_id)
                                next_states.append(state)
                            else:

                                ## CAN be SLIDING, or FLYING BACK
                                ## For Flying BACK. Flying Back is DANGEROUS, CAUSE LOOP

                                #required_direction_fly_back = self.get_required_direction(edge, entering_edge)
                                intersection_angle_1 = self.get_intersection_angle_from_directions(updated_direction[0], cal_angle(*self.vertices_of_edges[edge]))
                                intersection_angle_2 = self.get_intersection_angle_from_directions(updated_direction[1], cal_angle(*self.vertices_of_edges[edge]))

                                if (intersection_angle_1 != 0 and intersection_angle_1 != pi) or (intersection_angle_2 != 0 and intersection_angle_2 != pi):
                                    updated_bouncing_direction = self.update_reflection_angles(updated_direction, self.vertices_of_edges[edge])
                                    #print current_node, "current_direction: ", current_direction," bouncing direction:", updated_bouncing_direction 
                                    ## Do not allow jump between two surfaces (additional checked by isloop)
                                    state = (current_node, edge, updated_bouncing_direction, FLYING, True, updated_direction, obj_id)
                                    #if current_node == 40:
                                    #    print "note 40: state before bouncing: ", current_direction
                                    #    print "node 40: state after bouncing: ", state, "\n"

                                    next_states.append(state)

                                '''
                                if self.is_edge_surface[entering_edge] != 1:
                                state = (self.tri_neighbor_by_edge[current_node][entering_edge], entering_edge, required_direction_fly_back, FLYING)
                                next_states.append(state)                            
                                #print "current_node: ", current_node, "  flying back: ",  state
                                '''
                                ### SLIDING on this edge. It may not necessarily slide. if it cannot, it will be terminated at the next expanding anyway. can be changed later
                                ######### BUG AREA [Seems Fixed] ########################

                                connected_surfaces = self.surface_rel_dic[edge]
                                for surface_info in connected_surfaces:
                                   connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
                                   for angle in current_direction:
                                        if self.get_intersection_angle_from_directions(angle, on_surface_direction[0]) >= pi/2:
                                            continue
                                        else:
                                           state = (current_node, edge, on_surface_direction, SLIDING, True, updated_direction, obj_id)
                                           next_states.append(state)
                                           break

                                   '''

                                   for direction_range in current_direction:
                                       if not can_slide:
                                           for angle in direction_range:
                                                #if current_node == 32:
                                                #    print angle, " ", on_surface_direction, " ", self.get_intersection_angle_from_directions(angle, on_surface_direction[0][0])
                                                if self.get_intersection_angle_from_directions(angle, on_surface_direction[0][0]) >= pi/2:
                                                    continue
                                                else:
                                                   state = (current_node, edge, on_surface_direction, SLIDING, True, updated_direction, obj_id)
                                                   next_states.append(state)
                                                   can_slide = True
                                    '''

                                ######### BUG AREA ########################

                        else:
                            if self.edges_length[edge] > self.object_size:
                                #print self.tri_neighbor_by_edge, "  ", current_node
                                #for neighbor in neighbors:
                                #    print neighbor, "  ", graph.edges(current_node), " current_node: ", current_node
                                state = (self.tri_neighbor_by_edge[current_node][edge] , edge, updated_direction, FLYING, direction_change_flag, updated_direction, obj_id)
                                #print " next state: ", state
                                next_states.append(state)
                                #print "current_direction, ", current_direction, " required_direction: ", required_direction, " updated_direction: ", updated_direction, " new state: ", state


        elif current_motion == SLIDING:
            #print entering_edge
            #print self.is_edge_surface[entering_edge]
            ### IMPORTANT: NEED TO IMPLEMNT TRASITION FROM SLIDE TO FLY
            connected_surfaces = self.surface_rel_dic[entering_edge]                                 
            for surface_info in connected_surfaces:
                connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
                required_directions = self.get_required_direction_to_other_edges(connected_surface)

                #print "test sliding relation: ", current_node, "current direction: ", current_direction, " entering_direction: ", entering_direction, " required_directions: ", required_directions, " connected_tri: ", self.tri_by_surface[connected_surface], " can slide: ", self.can_slide(intersection_angle, required_directions[0],required_directions[1])
                #if current_node == 5:
                #    print current_direction, "  ", on_surface_direction
                if current_direction == on_surface_direction: 
                    #print "current_direction : ", current_direction, "  on_surface_direction: ", on_surface_direction
                    if self.can_slide(intersection_angle, required_directions[0],required_directions[1]):
                        state = (self.tri_by_surface[connected_surface], connected_surface, exiting_direction, SLIDING, True, current_direction , obj_id)
                        next_states.append(state)
                    
                    else:

                        ## IMPORTABNT, POTENTIAL BUG, the ball may only collide the connected surface when the surface is upwards in front of the ball
                        updated_bouncing_direction = self.update_reflection_angles(current_direction, self.vertices_of_edges[connected_surface])
                        #print current_node, "current_direction: ", current_direction," bouncing direction:", updated_bouncing_direction 
                        ## Do not allow jump between two surfaces (additional checked by isloop)
                        state = (current_node, connected_surface, updated_bouncing_direction, SLIDING, True, current_direction, obj_id)
                        #print "bouncing state: ", state
                        next_states.append(state)       

                        ## Add EVENT
                        #if current_node == 5:
                        #    print "before ", current_direction, " ", on_surface_direction 
                        #state = (current_node, entering_edge, current_direction, FLYING, False, current_direction, obj_id)
                        #if current_node == 5:
                        #    print " after ", state, current_direction
                        #next_states.append(state)
      


        #print " inferred states: ", next_states
        return next_states
def get_normal(v1, v2, v3):
    angle1 = cal_angle(v1, v2)
    angle2 = cal_angle(v2, v3)
    return (angle1 + angle2)/2