import base


class AStar():
    def __init__(self, mapborders):
        self.mapborders = mapborders
        self._initmap()

    
    def _initmap(self):
        # 计算边界的最大外围矩形
        minx = maxx = self.mapborders[0].start.x
        miny = maxy = self.mapborders[0].start.y
        for b in self.mapborders :
            tmpx = b.start.x
            tmpy = b.start.y
            if tmpx < minx : 
                minx = tmpx
            if tmpx > maxx : 
                maxx = tmpx
            if tmpy < miny : 
                miny = tmpy
            if tmpy > maxy : 
                maxy = tmpy

        # 从矩形起点（笛卡尔坐标系，x与y最小点），到矩形终点（笛卡尔坐标系，x与y最大点），以整数单位，逐个计算一个“是否在地图内”的矩阵点
        # [available, available,...,available
        # ...
        #  available, available,...,available]
        # msize/nsize: 矩阵的尺寸
        # xoffset/yoffset: 矩阵起点相对坐标原点的偏移量
        self.mapmatrx=[]
        for y in range(miny, maxy + 1) :              # +1，需要包含maxy在矩阵内
            oneline = []
            for x in range(minx, maxx + 1) :
                available = self._pointinmap(x, y)
                oneline.append(available)
            self.mapmatrx.append(oneline)
        self.nsize = maxx - minx + 1
        self.msize = maxy - miny + 1
        self.xoffset = minx
        self.yoffset = miny
        
    
    def _m2y(self, m):
        return m + self.yoffset
    def _n2x(self, n):
        return n + self.xoffset
    def _y2m(self, m):
        return m - self.yoffset
    def _x2n(self, n):
        return n - self.xoffset


    def _pointinmap(self, x, y):
        xccount = 0
        for b in self.mapborders :
            if b.AvailablePointInLine(base.Point(x, y)) :
                return 0
                
            xc = b.AvailablePointXCross(base.Point(x, y), 0)
            if xc is not None :
                xccount += 1
                
        if xccount%2 == 0 :
            return -1
        else:
            return 0

    
    def _printmatrx(self, array):
        for oneline in array :
            str = ""
            for v in oneline :
                str = "%s %2d" % (str, v)
            print(str)

    
            
    def FindWay(self, startpoint, endpoint, haveobstacle, obstaclestartpoint, obstaclewidth):
        # print("----------------start end info------------")
        # print("offset: %d %d, start: %d %d, endL %d %d" % (self.xoffset, self.yoffset, startpoint.x, startpoint.y, endpoint.x, endpoint.y))
        
        # print("----------------mapmatrx info------------")
        # self._printmatrx(self.mapmatrx)
        
        # 从矩阵中找到startpoint和endpoint，记录矩阵坐标
        startm = self._y2m(startpoint.y)
        startn = self._x2n(startpoint.x)
        endm = self._y2m(endpoint.y)
        endn = self._x2n(endpoint.x)
        startava = self.mapmatrx[startm][startn]
        endava = self.mapmatrx[endm][endn]
        if startava < 0 or endava < 0 :
            return None

        # 初始化gmatrx, hmatrx两个代价参数矩阵。地图内的点，设置为0；地图外的点，设置为-1
        gmatrx = self._copymatrx(self.mapmatrx)
        hmatrx = self._copymatrx(self.mapmatrx)
        
        # 如果有obstacle，在gmatrx和hmatrx中屏蔽掉obstacle占用的那些点
        if haveobstacle :
            obstaclestartm = self._y2m(obstaclestartpoint.y)
            obstaclestartn = self._x2n(obstaclestartpoint.x)
            self._addobstacle2matrx(gmatrx, obstaclestartm, obstaclestartn, obstaclewidth)
            self._addobstacle2matrx(hmatrx, obstaclestartm, obstaclestartn, obstaclewidth)

        # print("----------------gmatrx inited------------")
        # self._printmatrx(gmatrx) 

        # 遍历gmatrx和hmatrx中的有效点，设置与起始点的距离值，曼哈顿距离
        self._updatematrxcost(gmatrx, startm, startn)
        self._updatematrxcost(hmatrx, endm, endn)
        
        # print("----------------gmatrx caculated------------")
        # self._printmatrx(gmatrx)
        # print("----------------hmatrx caculated------------")
        # self._printmatrx(hmatrx)

        # 计算代价函数矩阵 fmatrx = gmatrx + hmatrx
        fmatrx = self._addmatrx(gmatrx, hmatrx)
        
        # print("----------------fmatrx caculated------------")
        # self._printmatrx(fmatrx) 
        
        # 从fmatrx矩阵中计算出路径集合
        waypositions = self._getwaypositions(startm, startn, endm, endn, fmatrx, hmatrx)
        '''
        print("------------way points------------")
        if waypositions is None:
            print("None")
        else:
            for p in waypositions:
                print(p.point.x, p.point.y, p.angle)
        '''
        return waypositions


    # 从起始点开始，向周围扩展，每扩展一步距离+1，更新周围点的距离后，将周围点记录成点集，再更新这个集合的周围点的距离，循环直至所有点更新完成
    def _updatematrxcost(self, matrx, m, n):
        openpoints = [[m, n]]
        distence = 1
        matrx[m][n] = distence
        while True :
            newopenpoints = []
            distence += 1
            for p in openpoints :
                m = p[0]
                n = p[1]
                if m > 0 :
                    if matrx[m-1][n] == 0 :
                        matrx[m-1][n] = distence
                        newopenpoints.append([m-1, n])
                if n > 0 :
                    if matrx[m][n-1] == 0 :
                        matrx[m][n-1] = distence
                        newopenpoints.append([m, n-1])
                if m < self.msize - 1 :
                    if matrx[m+1][n] == 0 :
                        matrx[m+1][n] = distence
                        newopenpoints.append([m+1, n])
                if n < self.nsize -1 :
                    if matrx[m][n+1] == 0 :
                        matrx[m][n+1] = distence
                        newopenpoints.append([m, n+1])

            if len(newopenpoints) > 0 :
                openpoints = newopenpoints
            else:
                break
        
        # 循环结束后，如果matrx里有cost仍是0的点，说明没有扩散到这些点，则把这些点标记为-1，表示不可达
        for m in range(0, self.msize) :
            for n in range(0, self.nsize) :
                if matrx[m][n] == 0 :
                    matrx[m][n] = -1
                    
    
    def _copymatrx(self, matrx):
        newmatrx = []
        for line in matrx :
            newline = []
            for elem in line :
                newline.append(elem)
            newmatrx.append(newline)
            
        return newmatrx
            
    
    def _addmatrx(self, matrxa, matrxb):
        matrx = []
        for m in range(0, self.msize) :
            line = []
            for n in range(0, self.nsize) :
                elema = matrxa[m][n]
                elemb = matrxb[m][n]
                if elema < 0 or elemb < 0 :
                    line.append(-1)
                else:
                    line.append(elema + elemb)
            matrx.append(line)
            
        return matrx

    
    def _addobstacle2matrx(self, matrx, obstaclestartm, obstaclestartn, obstaclewidth):
        obstacleendm = obstaclestartm + obstaclewidth
        obstacleendn =  obstaclestartn + obstaclewidth
        if obstaclestartm < 0:
            if obstacleendm <= 0 :
                return
            obstaclestartm = 0
        if obstacleendm > self.msize :
            if obstaclestartm >= self.msize :
                return
            obstacleendm = self.msize
        if obstaclestartn < 0:
            if obstacleendn <= 0 :
                return
            obstaclestartn = 0
        if obstacleendn > self.nsize :
            if obstaclestartn >= self.nsize :
                return
            obstacleendn = self.nsize
            
        for m in range(obstaclestartm, obstacleendm):
            for n in range(obstaclestartn, obstacleendn):
                matrx[m][n] = -1

    
    def _getwaypositions(self, startm, startn, endm, endn, fmatrx, hmatrx):
        waypositions = []
        
        curm = startm
        curn = startn
        curangle = -1
        loop = 0
        while loop < 1000 :
            loop += 1
            # 如果已经找到终点的邻居，终止路径查找
            if self._isneighbor(curm, curn, endm, endn) :
                tmpangle = self._getangle(curm, curn, endm, endn)
                if curangle == tmpangle :
                    waypositions.append(base.Position(base.Point(self._n2x(endn), self._m2y(endm)), curangle))
                else:
                    waypositions.append(base.Position(base.Point(self._n2x(curn), self._m2y(curm)), curangle))
                    waypositions.append(base.Position(base.Point(self._n2x(endn), self._m2y(endm)), tmpangle))
                return waypositions

            # 获取当前点的邻居点中代价值最小的点，作为路径的下一步。如果同时有两个相同代价值的点，则以angle不变优先
            (tmpm, tmpn, tmpangle) = self._getminneighbor(curm, curn, curangle, fmatrx, hmatrx)
            if tmpm < 0 :                               # 如果找不到有代价值的邻居点，则表示没找到目标点， 返回None
                return None
            elif curangle < 0 :                         # 如果当前angle无值（也就是起点），在起点上的转向不记为转向点
                curm = tmpm
                curn = tmpn
                curangle = tmpangle
            elif curangle == tmpangle :                 # 如果角度无变化，不记为转向点
                curm = tmpm
                curn = tmpn
            else:
                # print("%d %d %d" % (curn, curm, curangle))
                waypositions.append(base.Position(base.Point(self._n2x(curn), self._m2y(curm)), curangle))
                curm = tmpm
                curn = tmpn
                curangle = tmpangle


    def _isneighbor(self, pointx1, pointy1, pointx2, pointy2):
        distance = abs(pointx2 - pointx1) + abs(pointy2 - pointy1)            #曼哈顿距离
        if distance > 1 :
            return False
        else:
            return True
          

    def _getminneighbor(self, m, n, angle, fmatrx, hmatrx):
        minf = minm = minn = minangle = -1
        minh = -1
        if n > 0 and angle != 0 :               # 通过对进入当前节点的angle进行判断，把前一节点排除在neighbor之外（前一个节点虽然也是邻居点，但是不作为可选的下一跳节点）
            f = fmatrx[m][n-1]
            h = hmatrx[m][n-1]
            if f > 0:            # 判断neighbor节点是否有效的value
                if minf == -1 or minf > f or minh > h :
                    minf = f
                    minm = m
                    minn = n - 1
                    minh = h
                    minangle = 180
        if m > 0 and angle != 90 :
            f = fmatrx[m-1][n]
            h = hmatrx[m-1][n]
            if f > 0:
                if minf == -1 or minf > f or minh > h  : # 1、还没找到min点；2、新点比当前min点小
                    minf = f
                    minm = m - 1
                    minn = n
                    minh = h
                    minangle = 270
        if n < (self.nsize-1) and angle != 180:
            f = fmatrx[m][n+1]
            h = hmatrx[m][n+1]
            if f > 0 :
                if minf == -1 or minf > f or minh > h  :
                    minf = f
                    minm = m
                    minn = n + 1
                    minh = h
                    minangle = 0
        if m < (self.msize-1) and angle != 270:
            f = fmatrx[m+1][n]
            h = hmatrx[m+1][n]
            if f > 0 :
                if minf == -1 or minf > f or minh > h  :
                    minf = f
                    minm = m + 1
                    minn = n
                    minh = h
                    minangle = 90
        
        return (minm, minn, minangle)

    
    def _getangle(self, startm, startn, endm, endn):
        if startm > endm :
            return 270    # 矩阵与实际地图的y轴对称
        elif startm < endm :
            return 90
        elif startn > endn :
            return 180
        else :
            return 0