#!/usr/bin/python
#!encoding: utf-8
#author: Estefanía Pereyra
import random
import math
import pylab

def main(qStart, cGoalX, cGoalY, cLimits, cObsX, cObsY, kLimit):
    RRTV = []
    RRTE = []
    RRTV.append(qStart)
    RRTE.append(qStart)
    k = 0
    qNew = qStart
    while (((qNew[0][0] < cGoalX[0] or qNew[1][0] < cGoalX[0] or qNew[2][0] < cGoalX[0] or qNew[3][0]< cGoalX[0])
         or (qNew[0][0] > cGoalX[1] or qNew[1][0] > cGoalX[1] or qNew[2][0] > cGoalX[1] or qNew[3][0] > cGoalX[1])
         or (qNew[0][1] < cGoalY[0] or qNew[1][1] < cGoalY[0] or qNew[2][1] < cGoalY[0] or qNew[3][1] < cGoalY[0])
         or (qNew[0][1] > cGoalY[1] or qNew[1][1] > cGoalY[1] or qNew[2][1] > cGoalY[1] or qNew[3][1]> cGoalY[1]))
         and k < kLimit):
        
        if((qNew[0][0] > cGoalX[0] and qNew[0][0] < cGoalX[1]) and (qNew[0][1]  > cGoalY[0] and qNew[0][1] < cGoalY[1])):
            qRand = qRandom([[qNew[0][0]-1, qNew[0][0]+1],[qNew[0][1]-1, qNew[0][1]+1]], cObsX, cObsY)
       
        elif((qNew[1][0] > cGoalX[0] and qNew[1][0] < cGoalX[1]) and (qNew[1][1] > cGoalY[0] and qNew[1][1] < cGoalY[1])):
            qRand = qRandom([[qNew[1][0]-1, qNew[1][0]+1],[qNew[1][1]-1, qNew[1][1]+1]], cObsX, cObsY)
       
        elif((qNew[2][0] > cGoalX[0] and qNew[2][0] < cGoalX[1]) and (qNew[2][1] > cGoalY[0] and qNew[2][1] < cGoalY[1])):
            qRand = qRandom([[qNew[2][0]-1, qNew[2][0]+1],[qNew[2][1]-1, qNew[2][1]+1]], cObsX, cObsY)
       
        elif((qNew[3][0] > cGoalX[0] and qNew[3][0] < cGoalX[1]) and (qNew[3][1] > cGoalY[0] and qNew[3][1] < cGoalY[1])):
            qRand = qRandom([[qNew[3][0]-1, qNew[3][0]+1],[qNew[3][1]-1, qNew[3][1]+1]], cObsX, cObsY)
        else:
            qRand = qRandom(cLimits, cObsX, cObsY)
        
        extendRRT(RRTV, RRTE, qRand, cLimits, cObsX, cObsY)
        qNew = RRTV[-1] 
        qPrev = RRTE[-1]
        k += 1

    if k < kLimit:
        path = findPath(RRTV,RRTE)
        print 'k:', k
        plot(path, cLimits, cObsX, cObsY, cGoalX, cGoalY)
        return path
#        return True
    else:
        return False

def plot(path, cLimits, cObsX, cObsY, cGoalX, cGoalY):
    
    pylab.xlim([cLimits[0][0], cLimits[0][1]])
    pylab.ylim([cLimits[1][0], cLimits[1][1]])
     
    xg = [[cGoalX[0], cGoalX[1]], [cGoalX[1], cGoalX[1]], 
          [cGoalX[1], cGoalX[0]], [cGoalX[0], cGoalX[0]]]
    yg = [[cGoalY[0], cGoalY[0]], [cGoalY[0], cGoalY[1]], 
          [cGoalY[1], cGoalY[1]], [cGoalY[1], cGoalY[0]]]
    pylab.plot(xg, yg,'g')

    xo1 = [[cObsX[0][0], cObsX[0][1]], [cObsX[0][1], cObsX[0][1]], 
           [cObsX[0][1], cObsX[0][0]], [cObsX[0][0], cObsX[0][0]]]
    yo1 = [[cObsY[0][0], cObsY[0][0]], [cObsY[0][0], cObsY[0][1]], 
           [cObsY[0][1], cObsY[0][1]], [cObsY[0][1], cObsY[0][0]]]
    xo2 = [[cObsX[1][0], cObsX[1][1]], [cObsX[1][1], cObsX[1][1]], 
           [cObsX[1][1], cObsX[1][0]], [cObsX[1][0], cObsX[1][0]]]
    yo2 = [[cObsY[1][0], cObsY[1][0]], [cObsY[1][0], cObsY[1][1]], 
           [cObsY[1][1], cObsY[1][1]], [cObsY[1][1], cObsY[1][0]]]

    pylab.plot(xo1, yo1,'r')
    pylab.plot(xo2, yo2,'r')
      
    for i in xrange(len(path)):
        x1 = [path[i][0][0]]
        x2 = [path[i][1][0]]
        x3 = [path[i][2][0]]
        x4 = [path[i][3][0]]
        y1 = [path[i][0][1]]
        y2 = [path[i][1][1]]
        y3 = [path[i][2][1]]
        y4 = [path[i][3][1]]
        pylab.plot(x1,y1, 'bo')
        pylab.plot(x2,y2, 'ro')
        pylab.plot(x3,y3, 'go')
        pylab.plot(x4,y4, 'yo')

        if i != 0:
            xp1 = [path[i-1][0][0], path[i][0][0]]
            xp2 = [path[i-1][1][0], path[i][1][0]]
            xp3 = [path[i-1][2][0], path[i][2][0]]
            xp4 = [path[i-1][3][0], path[i][3][0]]
            yp1 = [path[i-1][0][1], path[i][0][1]]
            yp2 = [path[i-1][1][1], path[i][1][1]]
            yp3 = [path[i-1][2][1], path[i][2][1]]
            yp4 = [path[i-1][3][1], path[i][3][1]]
            pylab.plot(xp1,yp1,'b')
            pylab.plot(xp2,yp2,'r')
            pylab.plot(xp3,yp3,'g')
            pylab.plot(xp4,yp4,'y')

    pylab.show()
    
def findPath(RRTV, RRTE):
    path = []
    new = RRTV[-1]
    prev = RRTE[-1]
    path.append([[new[0][0],new[0][1],0.5],
                 [new[1][0],new[1][1],0.5],
                 [new[2][0],new[2][1],0.5],
                 [new[3][0],new[3][1],0.5]])
    
    while (new != RRTV[0]):
        for i in range(len(RRTV)):
            if(RRTV[i] == prev):
                new = prev
                prev = RRTE[i]
                path.append([[new[0][0],new[0][1],0.5],
                             [new[1][0],new[1][1],0.5],
                             [new[2][0],new[2][1],0.5],
                             [new[3][0],new[3][1],0.5]])
    path.reverse()
    return path

            
def qRandom (cLimits, cObsX, cObsY):
    qRand = [[random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
            [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
            [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])],
            [random.uniform(cLimits[0][0], cLimits[0][1]), random.uniform(cLimits[1][0],cLimits[1][1])]]
    
    for i in range(len(qRand)):
        if (((qRand[i][0] >= cObsX[0][0] and qRand[i][0] <= cObsX[0][1]) 
         and (qRand[i][1] >= cObsY[0][0] and qRand[i][1] <= cObsY[0][1])) 
         or ((qRand[i][0] >= cObsX[1][0] and qRand[i][0] <= cObsX[1][1]) 
         and (qRand[i][1] >= cObsY[1][0] and qRand[i][1] <= cObsY[1][1]))):
             qRand = qRandom(cLimits, cObsX, cObsY)
            
    return qRand

def extendRRT (RRTV, RRTE, qRand, cLimits, cObsX, cObsY):
    qNear, ind = nearestNeighbor(RRTV, qRand)
    qNew = newConfiguration(qNear, ind, qRand, RRTE)
    if (qNew !=0 and notCollisions(qNear, qNew, cLimits, cObsX, cObsY)):
        RRTV.append(qNew)
        RRTE.append(qNear)
    return 

def nearestNeighbor(RRTV, qRand):
    for i in range(len(RRTV)):
        if i == 0:
            dist = math.sqrt((RRTV[i][0][1]-qRand[0][1])**2 + 
                             (RRTV[i][0][0]-qRand[0][0])**2 + 
                             (RRTV[i][1][1]-qRand[1][1])**2 + 
                             (RRTV[i][1][0]-qRand[1][0])**2 + 
                             (RRTV[i][2][1]-qRand[2][1])**2 + 
                             (RRTV[i][2][0]-qRand[2][0])**2 + 
                             (RRTV[i][3][1]-qRand[3][1])**2 + 
                             (RRTV[i][3][0]-qRand[3][0])**2)

            qNear = RRTV[i]
            ind = i
        else:
            distAux = math.sqrt((RRTV[i][0][1]-qRand[0][1])**2 + 
                                (RRTV[i][0][0]-qRand[0][0])**2 + 
                                (RRTV[i][1][1]-qRand[1][1])**2 + 
                                (RRTV[i][1][0]-qRand[1][0])**2 + 
                                (RRTV[i][2][1]-qRand[2][1])**2 + 
                                (RRTV[i][2][0]-qRand[2][0])**2 + 
                                (RRTV[i][3][1]-qRand[3][1])**2 + 
                                (RRTV[i][3][0]-qRand[3][0])**2)
            
            if distAux < dist:
                dist = distAux
                qNear = RRTV[i]
                ind = i
    return qNear, ind

def newConfiguration(qNear, ind, qRand, RRTE):
    qNear1 = qNear
    qNear0 = RRTE[ind]
    theta1 = math.atan2(qNear1[0][1]-qNear0[0][1], qNear1[0][0]-qNear0[0][0])
    theta2 = math.atan2(qNear1[1][1]-qNear0[1][1], qNear1[1][0]-qNear0[1][0])
    theta3 = math.atan2(qNear1[2][1]-qNear0[2][1], qNear1[2][0]-qNear0[2][0])
    theta4 = math.atan2(qNear1[3][1]-qNear0[3][1], qNear1[3][0]-qNear0[3][0])
    step = 0.1
    qNewaux1 = []
    qNewaux2 = []
    qNewaux3 = []
    qNewaux4 = []
    delta = math.radians(-90)
    for i in range(3):
        qNewaux1.append([qNear[0][0] + step*math.cos(theta1+delta), qNear[0][1] + step*math.sin(theta1+delta)])
        qNewaux2.append([qNear[1][0] + step*math.cos(theta2+delta), qNear[1][1] + step*math.sin(theta2+delta)])
        qNewaux3.append([qNear[2][0] + step*math.cos(theta3+delta), qNear[2][1] + step*math.sin(theta3+delta)])
        qNewaux4.append([qNear[3][0] + step*math.cos(theta4+delta), qNear[3][1] + step*math.sin(theta4+delta)])
        delta += math.radians(90)

    nodos = []
    for i in range(len(qNewaux1)):
            for j in range(len(qNewaux2)):
                for k in range(len(qNewaux3)):
                    for l in range(len(qNewaux4)):
                        nodos.append([qNewaux1[i],qNewaux2[j],qNewaux3[k],qNewaux4[l]])
    nodosUtiles = formacion(nodos)
    if nodosUtiles != 0:
        qNew, ind = nearestNeighbor(nodosUtiles, qRand)
        return qNew
    else:
        return 0

def formacion(nodos):
    nodosvalidos = []
    n = 0
    for i in range(len(nodos)):
        norm = []
        norm.append(math.hypot(nodos[i][0][0] - nodos[i][1][0], nodos[i][0][1] - nodos[i][1][1]))
        norm.append(math.hypot(nodos[i][1][0] - nodos[i][2][0], nodos[i][1][1] - nodos[i][2][1]))
        norm.append(math.hypot(nodos[i][2][0] - nodos[i][3][0], nodos[i][2][1] - nodos[i][3][1]))
        norm.append(math.hypot(nodos[i][3][0] - nodos[i][0][0], nodos[i][3][1] - nodos[i][0][1]))
        norm.append(math.hypot(nodos[i][0][0] - nodos[i][2][0], nodos[i][0][1] - nodos[i][2][1]))
        norm.append(math.hypot(nodos[i][3][0] - nodos[i][1][0], nodos[i][3][1] - nodos[i][1][1]))
        if ((norm[0] < 1 and norm[0] > 0.49) and
            (norm[1] < 1 and norm[1] > 0.49) and
            (norm[2] < 1 and norm[2] > 0.49) and
            (norm[3] < 1 and norm[3] > 0.49) and
            (norm[4] < 1 and norm[4] > 0.49) and
            (norm[5] < 1 and norm[5] > 0.49)):
            nodosvalidos.append(nodos[i])
            n+=1
    if n == 0:
        return 0
    else:
        return nodosvalidos

def notCollisions(qNear, qNew, cLimits, cObsX, cObsY):
    qAux = []
    dx = []
    dy = []
    k = 0.25
    for i in range(len(qNear)):
        dx.append((qNew[i][0]-qNear[i][0])/10)
        dy.append((qNew[i][1]-qNear[i][1])/10)
    for j in range(10):
        qAux.append([[qNear[0][0] + dx[0]*j, qNear[0][1] + dy[0]*j],
                     [qNear[1][0] + dx[1]*j, qNear[1][1] + dy[1]*j],
                     [qNear[2][0] + dx[2]*j, qNear[2][1] + dy[2]*j],
                     [qNear[3][0] + dx[3]*j, qNear[3][1] + dy[3]*j]])
    qAux.append(qNew)
    for i in range(len(qAux)):
        for j in range(4):
            if (((qAux[i][j][0] >= (cObsX[0][0]-k) and qAux[i][j][0] <= (cObsX[0][1]+k)) 
                and (qAux[i][j][1] >= (cObsY[0][0]-k) and qAux[i][j][1] <= (cObsY[0][1]+k))) 
                or ((qAux[i][j][0] >= (cObsX[1][0]-k) and qAux[i][j][0] <= (cObsX[1][1]+k)) 
                and (qAux[i][j][1] >= (cObsY[1][0]-k) and qAux[i][j][1] <= (cObsY[1][1]+k)))):
                    return False 
    return True


#--Inicialización--
qStart = [[-2.25,-2.25],
          [-2.25,-1.75],
          [-1.75,-1.75],
          [-1.75,-2.25]]

cLimits = [[-2.5,2.5],
           [-2.5,2.5]]


cObsX = [[-2.5,-1.75],
        [0,2.5]]
cObsY = [[-0.5,0.5],
        [-0.5,0.5]]

cGoalX = [1.5,2.5]
cGoalY = [1,2]

klimit = 4000

if(not main(qStart, cGoalX, cGoalY, cLimits, cObsX, cObsY, klimit)):
    print 'Path not found'