def __init__(self, scenario_file, gravity_dir=(-1, -1)):

        self.count = 0
        self.gravity_dir = gravity_dir
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
            scenario_file
        )

        self.impulse = None
        # game_objects = self.__create_scenario__().getGameObjects()
        target_obj = self.__create_scenario__().b2_objects[self.target_obj["id"]]
        print target_obj.fixtures[0].massData.center
        self.target_pos = target_obj.transform * target_obj.fixtures[0].massData.center

        # self.target_pos = game_objects[self.target_obj['id']].mass_center
        print self.target_pos
import triangle.plot as plot
import matplotlib.pyplot as plt

# from scenario_generator import Scenario_Generator as SG
from game_object import GameObject as GOBJ
import numpy as np
import networkx as nx
from triangle_utils import triangle_center
from triangle_relation import printRels, getRel
from shapely.geometry import Polygon, LineString as Line
from math import pi, acos

file_name = "mini2"

scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj = loadScenario(
    "./scenarios/" + file_name + ".json"
)

scenario = SG(
    scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj, showRender=True
)
## get objects
game_objects = scenario.getGameObjects()

"""
dic = {}
dic["vertices"] = [[0.0,0.0],[100.0,0.0],[100.0,2.0],[0.0,2.0],[77.7722702026,4.0222196579],[99.0691375732,1.34814071655],[103.008628845,32.7230033875],[81.7117614746,35.3970832825],[33.6893234253,37.2380599976],[54.1199073792,32.9810180664],[55.028678894,37.3424224854],[34.5980949402,41.5994644165],[15.1510419846,52.2749481201],[36.719417572,55.3364334106],[36.4076156616,57.5330886841],[14.8392419815,54.4716033936],[55.4564323425,53.2923240662],[73.710144043,52.1638069153],[74.098487854,58.445224762],[55.8447723389,59.5737419128],[28.52501297,28.6967639923],[30.8609046936,28.6548023224],[31.2251396179,48.9308547974],[28.8892478943,48.9728164673],[17.6961288452,28.9960212708],[19.5502109528,28.8180770874],[21.4820976257,48.9470100403],[19.6280155182,49.1249542236],[59.7772750854,35.7274398804],[60.3249855042,35.7963409424],[58.2185592651,52.5418014526],[57.6708488464,52.4729003906],[38.7034988403,39.6549720764],[41.0576400757,40.1066627502],[39.6682815552,47.3478050232],[37.3141403198,46.8961143494],[34.6390304565,54.7437553406],[53.1063194275,52.5728187561],[53.4703216553,55.6692390442],[35.0030326843,57.8401756287],[41.0,40.0],[49.0,40.0],[49.0,42.0],[41.0,42.0],[0.0,0.0],[0.0,70.0],[100.0,70.0],[100.0,0.0]]

dic["segments"] = [[0,1],[1,2],[2,3],[3,0],[4,5],[5,6],[6,7],[7,4],[8,9],[9,10],[10,11],[11,8],[12,13],[13,14],[14,15],[15,12],[16,17],[17,18],[18,19],[19,16],[20,21],[21,22],[22,23],[23,20],[24,25],[25,26],[26,27],[27,24],[28,29],[29,30],[30,31],[31,28],[32,33],[33,34],[34,35],[35,32],[36,37],[37,38],[38,39],[39,36],[40,41],[41,42],[42,43],[43,40]]

dic["holes"] =  [[50.0,1.0],[90.3904495239,18.3726115227],[44.3590011597,37.2902412415],[25.7793288231,54.9040184021],[64.7774600983,55.8687744141],[29.8750762939,38.8138093948],[19.5891132355,38.9715156555],[58.9979171753,44.1346206665],[39.1858901978,43.5013885498],[44.0546760559,55.2064971924],[45.0,41.0],]
    def __init__(self, scenario_file, gravity_dir=(-1, -1)):
        # self.evalPath = EVAL(scenario_file)
        self.gravity_dir = gravity_dir
        self.evalPath = EVAL(scenario_file, gravity_dir=gravity_dir)
        # create scenario
        self.sigma = -0.0001
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
            scenario_file
        )
        self.target_obj_id = self.target_obj["id"]
        self.maxd = 3
        self.qualitative_paths = {}
        self.qualitative_paths_actions = {}
        self.zones = []

        self.simulation_counter = 0

        quali_sim = qualitative_simulation(scenario_file)
        self.estimated_qualitative_paths = quali_sim.run()

        self.initial_zone = quali_sim.initial_zone
        self.graph = quali_sim.graph
        self.zones = quali_sim.zones
        self.zone_dic = {}
        self.zone_distance = {}
        for x in xrange(self.width):
            for y in xrange(self.height):
                self.zone_dic[(x, y)] = self.__find_zones(x, y)

        for i in xrange(len(self.zones) - 1):
            self.zone_distance[(i, i)] = 0
            for j in xrange(i + 1, len(self.zones)):
                distance = self.zones[i].distance(self.zones[j])
                self.zone_distance[(i, j)] = distance
                self.zone_distance[(j, i)] = distance
                self.zone_distance[(len(self.zones) - 1, len(self.zones) - 1)] = 0
                
        else:
            while running:

                self.__sim_one_step__()
                if self.solved or self.__reach_end__():
                    break
            
            return self.solved



if __name__=="__main__":
    
     scenario_file = './scenarios/s5.json'
     test = Scenario_Generator(*loadScenario(scenario_file), showRender=True)
     #test.run()
     #test.apply_velocity_and_run()
     #print test.cart2pol(906.8805,0)
     test.apply_impulse_and_run((189.55180966738038, 0.7484582968892486))

     print test.solved
     #test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
     #print test.find_contacts_with_mobile_objs()
     #test.__sim_one_step__()
     #current_man = test.get_current_man()
     #test.run()
     #print  current_man.contacts[0].contact.worldManifold.points[0]
     #test.evaluate((972.8,0))
     #state = test.run()
     #print test.objects_distance_matrix()
 def __init__(self, scenario_file, gravity_dir=(-1, -1)):
     print "eval using: ", scenario_file, " gravity: ", gravity_dir
     self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
         scenario_file
     )
     self.count = 0
     self.first_sol = 0
     self.gravity_dir = gravity_dir
    def __init__(self, scenario_file):

    	# create scenario
    	self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
    	scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
    	game_objects = scenario.getGameObjects()
    	self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.zones = triangulate_advanced(game_objects, self.width, self.height)

        self.initial_zone = -1
        initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]

        initial_position = Point(initial_obj.position)
        for i in xrange(len(self.zones)):
            if self.zones[i].contains(initial_position):
                self.initial_zone = i
                break

        self.target_zones = []
        target_edges = self.edges_by_object_id[self.target_obj['id']]
        for edge in target_edges:
            #print "edge: ", edge
            if edge in self.edges_index:
                self.target_zones.append(self.tri_by_surface[edge])



        initial_edge = None
        for edge in self.edges_by_tri[self.initial_zone]:
            if self.is_on_surface(initial_obj, edge):
                initial_edge = edge
                break

        

        #self.target_zone = 8

        self.initial_obj_id = self.manipulatable_obj['id']
        self.target_obj_id = self.target_obj['id']

        initial_motion = "FLYING"
            
        self.initial_directions = []
        self.initial_states = []
        #path = simple_paths.next()

        for target_zone in self.target_zones:
            simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
            for path in simple_paths:        
                initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
                initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
                if initial_direction not in self.initial_directions:
                    self.initial_directions.append(initial_direction)
                    initial_state = (path[0], initial_edge, initial_direction, initial_motion, None, self.initial_obj_id) 
                    self.initial_states.append(initial_state)


        #print target_edge





        self.root_paths = {}

    	self.all_paths = []
    def __init__(self, scenario_file, perturb=False, gravity_dir=(-1,-1)):
        self.perturb = perturb
        self.gravity_dir = gravity_dir
        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
            scenario_file)
        self.scenario_file = scenario_file
        print 'solving scenario: ', scenario_file
        print 'gravity internal sim: ', gravity_dir

        self.evalReal = EvaluateQualiSol(scenario_file, gravity_dir)
        #self.evalReal = EvaluateQualiSol('./scenarios/mini2.json')
        #self.evalReal = EvaluateQualiSol('./scenarios/s2.json')
        self.objs_map = {}
        self.obj_min_angle_difference = {}
        self.first_real_sol = -1
        for obj in self.immobile_objs:
            if perturb:
                perturb_obj(obj)
            self.objs_map[obj['id']] = obj
            #self.obj_min_angle_difference[obj['id']] = [100,100]
            self.obj_min_angle_difference[obj['id']] = [100, 0]
        if perturb:
            sps(scenario_file, 5, self.immobile_objs)

        self.target_obj_id = self.target_obj['id']

        self.maxd = 3
        self.scenario = None
        self.qualitative_paths = {}
        self.qualitative_paths_actions = {}
        self.zones = []

        self.simulation_counter = 0
        self.round = 0
        quali_sim = qualitative_simulation(scenario_file)
        self.estimated_qualitative_paths = quali_sim.run()

        self.initial_zone = quali_sim.initial_zone
        self.graph = quali_sim.graph
        self.zones = quali_sim.zones
        self.zone_dic = {}
        self.zone_distance = {}
        for x in xrange(self.width):
            for y in xrange(self.height):
                self.zone_dic[(x, y)] = self.__find_zones(x, y)

        for i in xrange(len(self.zones) - 1):
            self.zone_distance[(i, i)] = 0
            for j in xrange(i + 1, len(self.zones)):
                distance = self.zones[i].distance(self.zones[j])
                self.zone_distance[(i, j)] = distance
                self.zone_distance[(j, i)] = distance
                self.zone_distance[(len(self.zones) - 1, len(self.zones) - 1)] = 0
    def __init__(self, scenario_file):

        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)

        #self.target_zone = 8 # hard code for debugging  s.6: 8, s6.1:12, s7:20
        self.target_obj_id = self.target_obj['id']
        self.maxd = 3

        self.qualitative_paths = {}
        self.qualitative_paths_actions = {}
        self.zones = []
        
        self.simulation_counter = 0

        quali_sim = qualitative_simulation(scenario_file)
        self.estimated_qualitative_paths_groups = quali_sim.run() 

        self.initial_zone = quali_sim.initial_zone       
        self.graph = quali_sim.graph
        self.zones = quali_sim.zones
    def __init__(self, target_region=None, O_i_rect=None, action=None, showRender=False, checksolution=False, scenario_file='s5.json'):
       
        super(Scenario_Generator, self).__init__()
        self.showRender = showRender
        self.state = -1
        self.min_d = 9999999
        self.state_impulse = (0,0)
       
        # Tell the framework we're going to use contacts, so keep track of them
        # every Step.
        self.using_contacts=True
        self.beginContact = False
        self.contactAID = -1
        self.contactBID = -1
        #current simulation round        
        self.round = 0
        self.objID = 0
        world = self.world

        self.objects = []

        self.game_objects = []

        self.target_region = target_region

        width, height, immobile_objs, mobile_objs, manipulatable_objs, target_obj = sr.loadScenario(scenario_file)

        for obj in immobile_objs:
            ## create a simulator object
            gobj = GOBJ(obj['id'])            
            if obj['shape'] == "box":
                static_body = world.CreateStaticBody(position=obj['position'], shapes=polygonShape(box=obj['size']), angle=obj['angle'],userData=gobj)
                                                
            elif obj['shape'] == "polygon":
                static_body = world.CreateStaticBody(position=obj['position'], shapes=polygonShape(vertices=obj['vertices']), userData=gobj)

            # store coordinates info in the screen
            mass_center = static_body.fixtures[0].massData.center            
            vertices=[(static_body.transform*v) for v in static_body.fixtures[0].shape]
            #vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
            gobj.setPosScr(vertices)
            gobj.setMassCenterSrc(static_body.transform * mass_center)
            
            self.game_objects.append(gobj)

        target_id = target_obj['id']

        for obj in mobile_objs:
            obj_id = obj['id']
         

            gobj = GOBJ(obj_id,role=GOBJ.MOVEABLE)
            if obj['shape'] == "box":
                dynamic_body = world.CreateDynamicBody(position=obj['position'], userData=gobj)
                dynamic_body.CreatePolygonFixture(box=obj['size'],density=obj['density'], friction=obj['friction'])
            
            mass_center = dynamic_body.fixtures[0].massData.center

            vertices=[(dynamic_body.transform*v)for v in dynamic_body.fixtures[0].shape]
            #vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
            gobj.setPosScr(vertices)
            gobj.setMassCenterSrc(dynamic_body.transform * mass_center)
            self.game_objects.append(gobj)


        for obj in manipulatable_objs:
            gobj = GOBJ(obj['id'],role=GOBJ.MAN)
            if obj['shape'] == "box":
                dynamic_body = world.CreateDynamicBody(position=obj['position'], userData=gobj)
                dynamic_body.CreatePolygonFixture(box=obj['size'],density=obj['density'], friction=obj['friction'])
            
            mass_center = dynamic_body.fixtures[0].massData.center

            vertices=[(dynamic_body.transform*v)for v in dynamic_body.fixtures[0].shape]
            #vertices=[(v[0], SCREEN_HEIGHT-v[1]) for v in vertices]
            gobj.setPosScr(vertices)
            gobj.setMassCenterSrc(dynamic_body.transform * mass_center)
            self.game_objects.append(gobj)
                
        '''
    def solve_with_rules_classify(self):
        self.round += 1
        all_paths = []
        classification = {}
        essential_contacts = set([])
        quali_paths = []
        sectors_score = []

        path_by_dir = {}
        path_bounces = {}
        path_first_bounces = {}
        possible_impulse_ranges = []
        for path_dir, path, essential_contact, bounce_pos_list in self.estimated_qualitative_paths:
            # print bounces_pos
            # heappush(all_paths, (len(bounces_pos), (path, bounces_pos)))
            # print path_dir
            comp_path = self.make_path_complete(path, self.graph)
            essential_contacts.add(essential_contact)
            if comp_path not in quali_paths:
                quali_paths.append(comp_path)

            if path_dir not in path_by_dir:
                path_by_dir[path_dir] = [comp_path]
                if len(bounce_pos_list) > 0:
                    path_bounces[path_dir] = [bounce_pos_list]
                    path_first_bounces[path_dir] = set([bounce_pos_list[0]])
                else:
                    path_first_bounces[path_dir] = set([])
                    path_bounces[path_dir] = []
            else:
                path_by_dir[path_dir].append(comp_path)
                if len(bounce_pos_list) > 0:
                    path_bounces[path_dir].append(bounce_pos_list)
                    path_first_bounces[path_dir].add(bounce_pos_list[0])

        sort_dirs = []
        for path_dir in path_bounces:
            # calculate average bounce distance
            bounce_pos_list = path_bounces[path_dir]
            average_distance = 0
            for bounce_pos in bounce_pos_list:
                total_distance = 0
                r = 0.6
                for i in xrange(len(bounce_pos) - 1):
                    total_distance += self.zone_distance[(bounce_pos[i], bounce_pos[i + 1])] * pow(1 + r, i)
                average_distance += total_distance
            if len(bounce_pos_list) == 0:
                average_distance = 0
            else:
                average_distance /= len(bounce_pos_list)
            print "path_dir: ", path_dir, "  ", average_distance

            heappush(sort_dirs, (average_distance, path_dir))
        '''
        while all_paths:
            path, bounces_pos = heappop(all_paths)
            print path, " bounces: ", bounces_pos
        '''
        while sort_dirs:
            # for path_dir in path_by_dir:
            distance, path_dir = heappop(sort_dirs)
            print "Test dir range: ", path_dir, " distance ", distance
            # bounces_count = 0

            # for bounces_pos in path_bounces[path_dir]:
            #    print bounces_pos
            # subdivide path_dir into 10 sectors
            # quali_paths = path_by_dir[path_dir]
            divided_sectors = self.divide_dir(path_dir)
            use_less_path = False
            num_iter = 15 #10  # 4
            detected_sols = []
            while num_iter > 0 and not use_less_path:
                num_iter -= 1
                bounces_count = 0

                for sector in divided_sectors:
                    num_samples = 10
                    impulse_range = (IMPULSE_RANGE_X, sector)
                    actions = sample_n_points_from_range(num_samples, impulse_range)
                    # print "test sector: ", sector
                    # print actions
                    for action in actions:
                        path, contacts_info, solved = self.find_qualitative_path_ptlike(action, self.initial_zone)
                        if solved:
                            print "solution in approx sim: ", action

                            real_traj, real_contacts, real_contacts_objs, real_solved = self.evalReal.trialshot_real(
                                action)
                            detected_sols.append(action)
                            if real_solved:
                                #print ' real: ', self.first_real_sol, '  not perturb: ', not self.perturb
                                if self.first_real_sol == -1:
                                    if not self.perturb:
                                        self.first_real_sol = self.simulation_counter
                                        print ' detect first real sol: ', action, self.simulation_counter
                                        exit()
                                    else:
                                        self.first_real_sol = self.evalReal.count

                                print "solution in real evnironment!!!!!", action

                            else:
                                # adjust
                                #self.adjust_approx_sim(real_traj, real_contacts, real_contacts_objs, action)
                                self.adjust_approx_sim_qualitative_path(real_traj, real_contacts, real_contacts_objs, action)

                            '''
                            real_qualitative_path = self.compute_qualitative_path(real_traj, self.initial_zone)
                            print "solution detected: ", action, "  ", self.simulation_counter
                            print "expected qualitative path: ", path, contacts_info
                            print "real qualitative path: ", real_qualitative_path, real_contacts
                            '''
                            continue

                        path = self.make_path_complete(path, self.graph)
                        for first_bounces in path_first_bounces[path_dir]:
                            if first_bounces in path:
                                bounces_count += bounces_count

                        for contact in contacts_info:
                            if contact[0] in essential_contacts:
                                path_str = str(self.make_path_complete(path, self.graph)).strip('[]')
                                # print "path after: " ,path
                                print "perturb_action: ", action
                                max_mu = self.perturb_action(action, path_str, essential_contact)
                                mu_list = np.random.normal(max_mu, 20, 100)

                                for mu in mu_list:
                                    _action = (mu, action[1])
                                    scenario = Scenario_Generator(self.width, self.height, self.immobile_objs,
                                                                  self.mobile_objs, self.manipulatable_obj,
                                                                  self.target_obj, showRender=False)
                                    scenario.apply_impulse_and_run(_action)
                                    self.simulation_counter += 1
                                    # print "sample action: ", _action
                                    if scenario.solved:
                                        print "solution detected in approx ", _action, " ", self.simulation_counter
                                        detected_sols.append(action)
                                        real_traj, real_contacts, real_contacts_objs, real_solved = self.evalReal.trialshot_real(
                                            action)
                                        if real_solved:
                                            if self.first_real_sol == -1:
                                                if not self.perturb:
                                                    self.first_real_sol = self.simulation_counter
                                                    print ' detect first real sol: ', self.simulation_counter
                                                    print exit()
                                                else:
                                                    self.first_real_sol = self.evalReal.count
                                            print "solution in real environment !!! ", action
                                            break
                                        else:
                                            # adjust
                                            #self.adjust_approx_sim(real_traj, real_contacts, real_contacts_objs, action)
                                            self.adjust_approx_sim_qualitative_path(real_traj, real_contacts, real_contacts_objs, action)
                                            break

                    '''
                    if bounces_count == 0:
                    print "exit at: ", 10 - num_iter
                    use_less_path = True
                    '''
            if len(detected_sols) >= 1:
                min_mu = 5000
                max_mu = 0
                min_angle = 7
                max_angle = 0
                for sol in detected_sols:
                    if sol[0] > max_mu:
                        max_mu = sol[0]
                    if sol[0] < min_mu:
                        min_mu = sol[0]
                    if sol[1] > max_angle:
                        max_angle = sol[1]
                    if sol[1] < min_angle:
                        min_angle = sol[1]
                if max_angle - max_angle < 0.1:
                    min_angle -= 0.1
                    max_angle += 0.1
                mu_range = (min_mu - 100, max_mu + 100)
                eval_impulse = [mu_range, (min_angle, max_angle)]
                possible_impulse_ranges.append(eval_impulse)

        for impulse_range in possible_impulse_ranges:
            print 'eval: ', impulse_range
            density, shots = self.evalReal.eval(1000, impulse_range)
            print 'density: ', density, 'first sol: ', shots , ' total shots: ', self.evalReal.count + shots
            if shots != -1 and self.first_real_sol == -1:
                self.first_real_sol = shots

        print 'num of trial shots: ', self.evalReal.count
        print 'simulation steps for finding the first sol: ', self.first_real_sol
        if self.first_real_sol == -1:
            if self.round >= 2:
                exit()
            gc.collect()
            self.simulation_counter = 0
            self.perturb = False
            self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(
                scenario_file)
            self.solve_with_rules_classify()


        if self.perturb:
            sps(self.scenario_file, 6, self.immobile_objs)
    def __init__(self, scenario_file):

        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
                    break

        else:
            while running:

                self.__sim_one_step__()
                if self.solved or self.__reach_end__():
                    break

            return self.solved


if __name__ == "__main__":
    # scenario_file = './scenarios/s3tmdmfms/4.json'
    scenario_file = './scenarios/mini2p5.json'
    test = Approx_Scenario_Generator(*loadScenario(scenario_file), sigma=-1, gravity_dir=(0,0), showRender=True)
    #test.run()
    # test.apply_velocity_and_run()
    # print test.cart2pol(906.8805,0)

    test.apply_impulse_and_run(((3294.3711984417114, 1.4930185179732731)))




    # print test.solved
    # test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
    # print test.find_contacts_with_mobile_objs()
    # test.__sim_one_step__()
    # current_man = test.get_current_man()
    # test.run()
                    break

        else:
            while running:

                self.__sim_one_step__()
                if self.solved or self.__reach_end__():
                    break

            return self.solved


if __name__ == "__main__":
    # scenario_file = './scenarios/s3tmdmfms/4.json'
    scenario_file = './scenarios/mini1.json'
    test = Scenario_Generator(*loadScenario(scenario_file), gravity_dir=(0,0), showRender=True)
    # test.run()
    # test.apply_velocity_and_run()
    # print test.cart2pol(906.8805,0)

    test.apply_impulse_and_run((376.63431624061826, 1.7420460328417813))
    traj, index = test.find_man_traj_bounce()

    print traj



    # print test.solved
    # test.apply_impulse_and_run((4517.779769295986, 0.11654142383307775))
    # print test.find_contacts_with_mobile_objs()
    # test.__sim_one_step__()
from scenario_generator import Scenario_Generator as SG
from game_object import GameObject as GOBJ
#from path_finding import createGraph
from triangulation import triangulate, triangulate_advanced
import numpy as np
import networkx as nx
from triangle_utils import triangle_center
from triangle_relation import printRels
from scenario_reader import loadScenario




#scenario_file = './scenarios/s3.json'
file_name = 's2'
scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj = loadScenario('./scenarios/' + file_name + '.json')
scenario = SG(scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj, showRender=True)
## get objects 
game_objects = scenario.getGameObjects()
tri = []
graph, edges_index, edges_dirs, edges_by_tri, vertices_of_edges, edges_surface_dic, tri_by_surface, surface_rel_dic, tri_neighbor_by_edge, edges_by_object_id, objects_id_by_edge, zones = triangulate_advanced(game_objects, scenario_width, scenario_height, tri)

plot.plot(plt.axes(),**(tri[0]))


################### Arrange graph according to the position of triangles #########

#triangles_by_vertices = tri["vertices"][tri["triangles"]]
pos = {}
for index, triangle in enumerate(zones):
    def __init__(self, scenario_file):

        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)

        #self.target_zone = 8 # hard code for debugging  s.6: 8, s6.1:12, s7:20
        self.target_obj_id = self.target_obj['id']
        self.maxd = 3

        self.qualitative_paths = {}
        self.qualitative_paths_actions = {}
        self.zones = []
        
        self.simulation_counter = 0

        quali_sim = qualitative_simulation(scenario_file)
        self.estimated_qualitative_paths = quali_sim.run() 

        self.initial_zone = quali_sim.initial_zone       
        self.graph = quali_sim.graph
        self.zones = quali_sim.zones
        self.zone_dic = {}
        self.zone_distance = {}
        for x in xrange(self.width):
            for y in xrange(self.height):
                self.zone_dic[(x,y)] = self.__find_zones(x,y)

        for i in xrange(len(self.zones)-1):
            self.zone_distance[(i,i)] = 0
            for j in xrange(i+1, len(self.zones)):
                distance = self.zones[i].distance(self.zones[j])
                self.zone_distance[(i,j)] = distance
                self.zone_distance[(j,i)] = distance
                self.zone_distance[(len(self.zones)-1, len(self.zones)-1)] = 0
    def __init__(self, scenario_file):

        # create scenario
        
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
        scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
        game_objects = scenario.getGameObjects()

        self.b2_objects = scenario.b2_objects
              
        ## draw triangulation
        # tri = [] may cause segmentation fault
        tri = None
        ###
        self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.edges_length, self.zones = triangulate_advanced(game_objects, self.width, self.height, tri)
        #triangulate_advanced(game_objects, self.width, self.height, tri)
        #triangulate_advanced(game_objects, self.width, self.height, tri)

        #dt(self.graph, tri, self.zones)


        self.initial_zone = -1
        initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]

        self.object_size = 16

        initial_position = Point(initial_obj.position)
        for i in xrange(len(self.zones)):
            if self.zones[i].contains(initial_position):
                self.initial_zone = i
                break

        self.target_zones = []

        target_edges = self.edges_by_object_id[self.target_obj['id']]
        for edge in target_edges:
            #print "edge: ", edge
            if edge in self.edges_index:
                self.target_zones.append(self.tri_by_surface[edge])



        self.initial_edge = None
        min_d = 99999

        for edge in self.edges_by_tri[self.initial_zone]:
            flag, distance = self.is_on_surface(initial_obj, edge)
            if flag:
                self.initial_edge = edge
                break
            else:
                if min_d > distance:
                    min_d = distance
                    self.initial_edge = edge
           


        

        #self.target_zone = 8

        self.initial_obj_id = self.manipulatable_obj['id']
        self.target_obj_id = self.target_obj['id']

        initial_motion = "FLYING"
            
        self.initial_directions = []
        self.initial_states = []
        #path = simple_paths.next()
        self.initial_position = self.b2_objects[self.manipulatable_obj['id']].position
        for target_zone in self.target_zones:

            simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
            for path in simple_paths:        
                initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
                #initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
                ########################## DEBUG  added #######################
                initial_direction = self.compute_initial_direction_exact_position(self.initial_position, self.vertices_of_edges[initial_target_edge])
                if initial_direction[0] < 0:
                    initial_direction = (0, initial_direction[1])
                ############################ ###################################################
                if initial_direction not in self.initial_directions:
                    self.initial_directions.append(initial_direction)
                    ### state = (current zone id, current edge index set, direction:[(),()], motion_type, collision happened, direction before updated, current obj id)

                    initial_state = (path[0], self.initial_edge, initial_direction, initial_motion, False , None, self.initial_obj_id) 
                    
                    self.initial_states.append(initial_state)

                    ### check if can slide:
                    if self.is_edge_surface[self.initial_edge] == 1:
                        connected_surfaces = self.surface_rel_dic[self.initial_edge]
                        for surface_info in connected_surfaces:
                            connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info

                            if on_surface_direction[0] <= initial_direction[1] and on_surface_direction[0] >=initial_direction[0]:
                                initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id)
                                #print "add initial state ", initial_state
                                #self.initial_states.append(initial_state)
                    '''
                    if self.is_edge_surface[self.initial_edge] == 1:
                        connected_surfaces = self.surface_rel_dic[self.initial_edge]
                        for surface_info in connected_surfaces:
                            connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
                            if self.tri_by_surface[connected_surface] == path[1]:
                                initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id )

                                # add event
                                self.initial_states.append(initial_state)
                    '''
                    ##### Can remove later




        #print self.edges_by_object_id[self.target_obj_id]



        self.root_paths = {}

        self.all_paths = []
                if save_image and self.round == 1: # without round = 100, some objects will not be settled when taking screenshots
                    pygame.image.save(screen, 's1.png')
                    print ("screenshot was taken")
                    save_image = False

                self.__sim_one_step__()
                if self.solved or self.__reach_end__():                    
                    break
                
        else:
            while running:

                self.__sim_one_step__()
                if self.solved or self.__reach_end__():
                    break
            
            return self.solved

if __name__=="__main__":
    
     scenario_file = './scenarios/s7.json'
     test = sim_object_behaviour(*loadScenario(scenario_file), showRender=True)
     test.apply_impulse_obj(4, (100, 0.0),(-1,-4))
     test.run()
     #test.evaluate((972.8,0))
     #state = test.run()
     #print test.objects_distance_matrix()

     

    def __init__(self, scenario_file):

        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
        self.target_zone = 12 # hard code for debugging eventual_goal:8 s.6: 12, s6.1:8, s7:20
        self.target_obj_id = self.target_obj['id']
        self.initial_zone = 24 # hard code for debugging s.6:24, s6.1:4, s7:3
        self.maxd = 3

        self.qualitative_paths = {}
        self.qualitative_paths_actions = {}
        self.zones = []
        self.graph = None
        self.simulation_counter = 0
    def __init__(self, scenario_file):

        # create scenario
        self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
        self.target_zone = 12 # hard code for debugging eventual_goal:8
        self.initial_zone = 24 # hard code for debugging
        self.maxd = 3