class Astar:
    def __init__(self, control):
        self.control = control
        self.gps = Algorithms(self.control)
        self.opened = []
        heapq.heapify(self.opened)
        self.closed = set()
        self.cells = []
        self.walls = []
        self.grid_height = 9  # size of the y-axis
        self.grid_width = 9  # size of the x-axis

    def create_walls(self):
        """
        walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3),  # This is a test array should be filled from sensor data
                 (3, 1), (3, 2), (3, 5), (4, 1), (4, 4), (5, 1))

        this should be filled with database requests for all information about the grid area.
        then any obstacles should be added the the walls array
        """

        self.walls = [
            Wall(0, 5, 5),
            Wall(1, 0, 5),
            Wall(1, 1, 5),
            Wall(1, 5, 5),
            Wall(2, 3, 5),
            Wall(3, 1, 5),
            Wall(3, 2, 5),
            Wall(3, 5, 5),
            Wall(4, 1, 5),
            Wall(4, 4, 5),
            Wall(5, 1, 5),
        ]

        return self.walls

    # need to give the grids there lat and long positions.
    def init_grid(self, startlocation, endlocation):
        walls = self.create_walls()
        # mkae a variable so it relates to the size of the
        bottom_left_location_lat = math.floor(self.grid_height / 2) * 0.00001
        bottom_left_location_lon = math.floor(self.grid_width / 2) * 0.00001
        lat = startlocation[0] - bottom_left_location_lat  # one more than the actual distance as it gets inc'ed first
        lon = startlocation[1] - bottom_left_location_lon  # one more than the actual distance as it gets inc'ed first
        for x in range(self.grid_width):
            lon += 0.00001
            for y in range(self.grid_height):
                wall = self.find_wall(x, y)
                if isinstance(wall, Wall):
                    reachable = wall.accessibility
                else:
                    reachable = 1
                lat += 0.00001

                self.cells.append(Cell(x, y, reachable, lat, lon))

        self.start = self.get_cell(startlocation[0], startlocation[1])
        self.end = self.get_cell(endlocation[0], endlocation[1])

    def get_heuristic(self, cell):
        """
        compute the heuristic value H for a cell
        distance between this cell anf the end cell multiplied by 10

        :param cell:
        :return heuristic value:
        """
        return 10 * (abs(cell.x - self.end.x) + abs(cell.y - self.end.y))

    def find_wall(self, x, y):
        for wall in self.walls:
            if wall.x == x and wall.y == y:
                return wall
        return False

    def get_cell(self, x, y):
        """
        returns a cell by using the x and y coordinates to find it in the array cells
        :param x:
        :param y:
        :return:
        """
        return self.cells[x * self.grid_height + y]

    def get_adjacent_cells(self, cell):
        """
        Returns adjacent cells to a cell. Clockwise starting
        from the one on the right.

        @param cell get adjacent cells for this cell
        @returns adjacent cells list
        """
        cells = []
        if cell.x < self.grid_width - 1:
            cells.append(self.get_cell(cell.x + 1, cell.y))
        if cell.y > 0:
            cells.append(self.get_cell(cell.x, cell.y - 1))
        if cell.x > 0:
            cells.append(self.get_cell(cell.x - 1, cell.y))
        if cell.y < self.grid_height - 1:
            cells.append(self.get_cell(cell.x, cell.y + 1))
        return cells

    def display_path(self):
        cell = self.end
        while cell.parent is not self.start:
            cell = cell.parent
            print "path: cell: %d,%d" % (cell.x, cell.y)

    def update_cell(self, adj, cell):
        """
        Update adjacent cell

        @param adj adjacent cell to current cell
        @param cell current cell being processed
        """
        adj.g = (cell.g + 10) + (cell.reachable * 10)  # better but needs to be tested
        adj.h = self.get_heuristic(adj)
        adj.parent = cell
        adj.f = adj.h + adj.g

    def get_path(self):
        cell = self.end
        path = []
        while cell.parent is not self.start:
            path.append(cell)
            cell = cell.parent
            print "path: cell: %d, %d, %d. %d" % (cell.x, cell.y, cell.lat, cell.lon)
        return path

    def solve(self):
        """Solve maze, find path to ending cell.
        @returns path or None if not found.
        """
        # add starting cell to open heap queue
        heapq.heappush(self.opened, (self.start.f, self.start))
        while len(self.opened):
            # pop cell from heap queue
            f, cell = heapq.heappop(self.opened)
            # add cell to closed list so we don't process it twice
            self.closed.add(cell)
            # if ending cell, return found path
            if cell is self.end:
                return self.get_path()
            # get adjacent cells for cell
            adj_cells = self.get_adjacent_cells(cell)
            for adj_cell in adj_cells:
                if adj_cell.reachable < 4 and adj_cell not in self.closed:
                    if (adj_cell.f, adj_cell) in self.opened:
                        # if adj cell in open list, check if current path is
                        # better than the one previously found
                        # for this adj cell.
                        if adj_cell.g > cell.g + 10:
                            self.update_cell(adj_cell, cell)
                    else:
                        self.update_cell(adj_cell, cell)
                        # add adj cell to open list
                        heapq.heappush(self.opened, (adj_cell.f, adj_cell))

    def get_grid_data(self, destination):
        curlocation = self.gps.getLocation()

        distance = self.gps.getDistance(curlocation, destination)
        bearing = self.gps.getHeading(curlocation, destination)

        grid_height = abs(self.grid_height)
        grid_width = abs(self.grid_width)

        calc_width = 0
        calc_height = 0

        centre_width = math.floor(self.grid_width / 2)
        centre_height = math.floor(self.grid_height / 2)
        startlocation = [centre_width, centre_height]

        if distance < centre_height:
            if 0 < bearing < 180:
                if bearing < 90:
                    calc_width = centre_width + math.floor(distance * math.sin(bearing))
                    calc_height = centre_height + math.floor(distance * math.cos(bearing))
                else:
                    calc_width = centre_width + math.floor(distance * math.sin(180 - bearing))
                    calc_height = centre_height - math.floor(distance * math.cos(180 - bearing))
            else:
                if bearing > -90:
                    calc_width = centre_width - math.floor(distance * math.cos(math.fabs(bearing)))
                    calc_height = centre_height + math.floor(distance * math.cos(math.fabs(bearing)))
                else:
                    calc_width = centre_width - math.floor(distance * math.cos(math.fabs((-180) - bearing)))
                    calc_height = centre_height - math.floor(distance * math.cos(math.fabs((-180) - bearing)))

        else:
            if 0 < bearing < 180:
                if 0 < bearing < 90:
                    if bearing < 45:
                        calc_width = centre_width + math.floor(centre_height * math.tan(math.fabs(bearing)))
                        calc_height = grid_height
                    else:
                        calc_width = grid_width
                        calc_height = centre_height + math.floor(centre_width / math.tan(math.fabs(bearing)))
                else:
                    if bearing < 135:
                        calc_width = grid_width
                        calc_height = centre_height - math.floor(centre_width * math.tan(math.fabs(bearing - 90)))
                    else:
                        calc_width = centre_width + math.floor(centre_height / math.tan(math.fabs(bearing - 90)))
                        calc_height = 0
            else:
                if -90 < bearing < 0:
                    if bearing > -45:
                        calc_width = centre_width - math.floor(centre_height * math.tan(math.fabs(bearing)))
                        calc_height = grid_height
                    else:
                        calc_width = 0
                        calc_height = centre_height + math.floor(centre_width / math.tan(math.fabs(bearing)))
                else:
                    if bearing > -135:
                        calc_width = 0
                        calc_height = centre_height - math.floor(centre_width * math.tan(math.fabs(bearing + 90)))
                    else:
                        calc_width = centre_width - math.floor(centre_height / math.tan(math.fabs(bearing + 90)))
                        calc_height = 0
        endlocation = [calc_width, calc_height]

        return [curlocation, distance, bearing, grid_width, grid_height, startlocation, endlocation]

    @database_grid_update
    def run_astar(self, destination):
        # 0 - curlocation, 1 - distance, 2 - bearing, 3 - grid_width,
        # 4 - grid_height, 5 - startlocation, 6 - endlocation

        destination = format(destination, ".5f")
        grid_values = self.get_grid_data(destination)

        curlocation = grid_values[0]
        distance = grid_values[1]
        bearing = grid_values[2]

        self.grid_width = grid_values[3]
        self.grid_height = grid_values[4]
        startlocation = grid_values[5]
        endlocation = grid_values[6]

        startlocation = format(startlocation, ".5f")
        endlocation = format(endlocation, ".5f")

        # this is where data is read from the database to fill the grid with non reachable locations
        self.create_walls()

        # build the grid using including the walls created by the database data
        self.init_grid(startlocation, endlocation)

        # find a path through the current grid
        self.solve()
        path = self.get_path()
        points = []

        # build points
        for i in range(0, len(path), 1):
            points.append([path[i].lat, path[i].lon])

        self.gps.followPath(points, 50)

        # get the new current location after following the path
        curlocation = format(self.gps.getLocation(), ".5f")

        # check if the destination has been reached.
        if curlocation != destination:
            print "Current location is : {0}.\nRe-running A-Star to get to Destination : {1}".format(
                str(curlocation), str(destination)
            )
            self.run_astar(destination)

        print "A-Star complete, with current location : {0}.\nThe given destination was : {1}".format(
            str(curlocation), str(destination)
        )

        return
        latitude.append(sys.argv[i])
        longitude.append(sys.argv[i + 1])
        points.append([latitude[j], longitude[j]])
        j += 1

    speedpercent = int(sys.argv[sys.argv.__len__() - 1])

    print 'Speed: ' + str(speedpercent)
    print 'lat: ' + str(latitude)
    print 'long: ' + str(longitude)
    print 'Points: ' + str(points)

    if algo is '0':
        g.pointToPoint([latitude[0], longitude[0]], speedpercent)
    elif algo is '1':
        g.followPath(points, speedpercent)
except Exception as e:
    print e
    '''
    print 'Well that didn\'t quite go to plan!!!'
    print 'Did you run this script correctly?'
    print 'Hmmm maybe I can help you out with that, being a supreme being and all!'
    print 'Try using this format:'
    print 'python test_gps_algorithms.py algo lat1 long1 lat2 long2 ... check speedpercent'
    print '\n'
    print 'The algo argument is used to select which algorithm is being tested:'
    print '0 - pointToPoint, 1 - followPath'
    print 'The number of lat and long values is up to you but they must have the same number of values and'
    print 'If you select pointToPoint then the code will only consider the first values'
    print 'The check argument is the number of meters between each check on if tiberius is going to the right place'
    print 'And finally speedpercent is the speed percentage of tiberius, hint its a percentage so between 0 and 100 :)'