import math

import utilities
reload(utilities)
from utilities import *

import Map
reload(Map)
from Map import *

# Three dimensional configuration-space grid.  Just used, for now, to figure
# out what space is free.  Takes a map, the x and y coordinate ranges,
# and a granularity for the grid.
class GridMap:
    robotWidth = 0.2
    
    def __init__(self, theMap, xmin, xmax, ymin, ymax, gridN):

        self.xmin = float(xmin)
        self.xmax = float(xmax)
        self.ymin = float(ymin)
        self.ymax = float(ymax)
        self.gridN = gridN
        self.xstep = (self.xmax - self.xmin)/self.gridN
        self.ystep = (self.ymax - self.ymin)/self.gridN
        self.thetaMin = 0.0
        self.thetaMax = 2.0 * math.pi
        self.thetaStep = (self.thetaMax - self.thetaMin)/self.gridN
        self.map = theMap

        # Is the grid cell centered at world coordinates (x,y) inside
        # the box?  It's not enough to see if the point (x,y) is in
        # the box: we have to check whether or not any part of the
        # whole cell is in it.  Furthermore, we have to increase the
        # cell size by the radius of the robot (we're assuming it's
        # round).
        def contains((x1,y1,x2,y2),x,y,margin):
            xmin = min(x1,x2)
            ymin = min(y1,y2)
            xmax = max(x1,x2)
            ymax = max(y1,y2)
            return x >= xmin - self.xstep/2.0 - margin and \
                   x <= xmax + self.xstep/2.0 + margin and \
                   y >= ymin - self.ystep/2.0 - margin and \
                   y <= ymax + self.ystep/2.0 + margin

        # Is the grid cell whose center is at real-world coordinates
        # (x,y) occupied in this map?
        def occupied(x,y,obstacleBoxes):
            for obstacle in obstacleBoxes:
                if contains(obstacle,x,y,self.robotWidth):
                    return True
            return False

        self.freeSpace = make2DArray(self.gridN, self.gridN, 1)
        for xIndex in range(self.gridN):
            for yIndex in range(self.gridN):
                (px,py) = self.indicesToPose2((xIndex,yIndex))
                if occupied(px,py,theMap.boxes):
                    self.freeSpace[xIndex][yIndex] = 0

    def free(self, (i,j)):
        return i >= 0 and i < self.gridN and j >= 0 and j < self.gridN \
               and self.freeSpace[i][j] == 1
    
    def xToIndex(self, x):
        return min(int(round(((x-self.xmin)/(self.xmax-self.xmin)) \
                             *self.gridN)),self.gridN-1)
    def yToIndex(self, y):
        return min(int(round(((y-self.ymin)/(self.ymax-self.ymin))  \
                             *self.gridN)),self.gridN-1)
    def thToIndex(self, theta):
        return min(int(round(((theta-self.thetaMin) \
                              /(self.thetaMax-self.thetaMin)) \
                             *self.gridN)),self.gridN-1)

    def poseToIndices(self, (x,y,theta)):
        return(self.xToIndex(x),self.yToIndex(y),self.thToIndex(theta))

    def indicesToPose(self, (ix,iy,itheta)):
        return(self.xmin+(float(ix)*(self.xmax-self.xmin)/self.gridN),
               self.ymin+(float(iy)*(self.ymax-self.ymin)/self.gridN),
               self.thetaMin+(float(itheta)*(self.thetaMax-self.thetaMin)\
                              /self.gridN))
    
    def poseToIndices2(self, (x,y)):
        return(self.xToIndex(x),self.yToIndex(y))

    def indicesToPose2(self, (ix,iy)):
        return(self.xmin+(float(ix)*(self.xmax-self.xmin)/self.gridN),
               self.ymin+(float(iy)*(self.ymax-self.ymin)/self.gridN))

    # Takes an angle index and return 0,1,2,3 (N, E, S, W)
    def compassDir(self, itheta):
        eighthRange = self.gridN / 8.0
        if itheta < eighthRange:
            return 1  # east
        elif itheta < 3*eighthRange:
            return 0  # north
        elif itheta < 5*eighthRange:
            return 3 # west  
        elif itheta < 7*eighthRange:
            return 2 # south
        else:
            return 1 # east

    # Takes an angle index and return 0,1,2,3 (
    def compassDirToIndex(self, idir):
        if idir == 0:  # north
            return self.gridN / 4
        elif idir == 1:  # east
            return 0
        elif idir == 2:  # south
            return self.gridN * 3 / 4
        else:  # west
            return self.gridN / 2

    def indicesAndDirToPose(self, (ix, iy, idir)):
        return self.indicesToPose((ix, iy, self.compassDirToIndex(idir)))

    def poseToIndicesAndDir(self, pose):
        indices = self.poseToIndices(pose)
        return (indices[0], indices[1],self.compassDir(indices[2]))

    def draw(self, window):
        size = int(math.floor(window.windowWidth/float(self.gridN)))-4
        window.clear()
        x = self.xmin
        for xIndex in range(self.gridN):
            y = self.ymin
            for yIndex in range(self.gridN):
                if not self.free((xIndex,yIndex)):
                    window.drawRobot(x, y, x, y, color = "black",
                                     size = size)
                else:
                    window.drawRobot(x, y, x, y, color = "white",
                                     size = size)
                y = y + self.ystep
            x = x + self.xstep
        # Draw the map boxes
        self.map.draw(window)
