import sys
import cv2
import numpy as np
import networkx as nx
from sklearn.neighbors import NearestNeighbors
import matplotlib.pyplot as plt
import math
import pylab
sys.setrecursionlimit(10000)
i=0

# CLASS POINT
class Point:
	def __init__(self,x,y):
		self.x=x
		self.y=y
# CLASS OBSTACLE
class Obst:
	def __init__(self,p1,p2):
		self.p1=p1
		self.p2=p2
		self.slopey=(p2.y-p1.y)
		self.slopex=(p2.x-p1.x)
#	def printo(self):
#		print "(",self.p1.x,",",self.p1.y,") to (",self.p2.x,",",self.p2.y,")"

# Check the side on which a point lies
def ccw(A,B,C):
    return (C.y-A.y) * (B.x-A.x) > (B.y-A.y) * (C.x-A.x)

# Return true if line segments AB and CD intersect
def intersect(A,B,C,D):
    return ccw(A,C,D) != ccw(B,C,D) and ccw(A,B,C) != ccw(A,B,D)

#convert the angle data into points
def makeconfig(state,dof):
	config = [[0]*2 for x in range(dof+1)]
	teta = 0
	config[0][0] = 0
	for i in range(1,dof+1):
		angle = math.radians(state[i-1])+teta
		config[i][0] = config[i-1][0] + arms[i-1]*math.cos(angle)
		config[i][1] = config[i-1][1] + arms[i-1]*math.sin(angle)
		teta = angle
	return config
#check whether a configuration intersects any obstacle
def checkobs(obstacles,state,dof):
	config = makeconfig(state,dof)
	for i in range(1,dof+1): #traversing all arms
		p1= Point(config[i-1][0],config[i-1][1]) #marking the end points
		p2= Point(config[i][0],config[i][1])
		for n in range(0,numobs):  # to check with all obstacles
			obsta = obstacles[n]
			if intersect(obsta.p1,obsta.p2,p1,p2):
				return 1
	return 0

### START READING THE INPUT

flag=0 # TO CAPTURE OBSTACLE POINTS
# READING THROUGH INPUT FILE
for line in open("robot2.dat", 'r'):
    item = line.rstrip() # strip off newline and any other trailing whitespace
    item = item.split()
    if len(item)==0: continue  # IF THE LINE IS BLANK LEAVE IT AND MOVE AHEAD
    item = [int(x) for x in item]
    i=i+1
    if i==1: dof=item[0];arms = [0]*dof      # FIRST LINE OF INFORMATION HAS THE DOF
    elif i==2:                               # SECOND LINE OF INFORMATION HAS ARM LENGTHS
	for k in range(0,dof):
	  arms[k] = item[k]
    elif i==3: numobs = item[0];obstacles = [Obst]*numobs;obs=0         #THIRD LINE OF INFORMATION HAS OBSTACLE-COUNT
    elif flag==0: p1=Point(item[0],item[1]);flag=1			#ALL REMAINING LINES HAVE INFORMATION ABOUT OBSTACLES
    elif flag==1: p2=Point(item[0],item[1]);flag=0;obstacles[obs] = Obst(p1,p2);obs=obs+1
#print "dof=",dof,"armlengths=",arms,"numobs=",numobs
#for obstacle in obstacles:
#	obstacle.printo()

space = 10000
theta = [[0]*dof for x in range(space)]
out = [""]*space
#READING GOAL MATRIX
i=0
j=0
for line in open("goals2.dat", 'r'):
	item = line.rstrip()
	item = item.split()
	item = [float(x) for x in item]
	if checkobs(obstacles,item,dof):
		print item,"has an obstacle intersection";continue
	i=i+1
	if i==1: theta[0] = item;out[0] = [str(ab) for ab in theta[0]];out[0] = " ".join(out[0])
	else:
		for p in range(0,dof):
			if item[p]<0: item[p]= 360+item[p]
		theta[j+1] = item;out[j+1] = [str(ab) for ab in theta[j+1]];out[j+1] = " ".join(out[j+1]);j=j+1
ngoals = j

i=ngoals+1

# GENERATING RANDOM POINTS FOR C - SPACE
from random import *
while i<space:
	for j in range(0,dof):
		theta[i]=[360*random() for x in range(dof)]
		out[i] = [str(ab) for ab in theta[i]]
		out[i] = " ".join(out[i])
	if checkobs(obstacles,theta[i],dof):
		i=i-1
	i=i+1
out = "\n".join(out)
fp = open("cspace.txt","w")
fp.write(out)
def dist(state1,state2):
	sum1 =0
	for i in range(0,dof):
		diff=abs(state1[i]-state2[i])
		if diff>180:
			diff=360-diff
		sum1 = sum1 + diff*diff
	return math.sqrt(sum1)

k=10
#FINDING NEAREST NEIGHBOURS
nbrs = NearestNeighbors(n_neighbors=k, algorithm='ball_tree').fit(theta)
distances, indices = nbrs.kneighbors(theta)
#print indices[0]
#print distances[0]
# NOW THE MAIN PROCESSING BEGINS
def midpt(state1,state2):
	state = [0 for x in range(dof)]
	for i in range(0,dof):
		diff = abs(state2[i]-state1[i])
		#if diff > 180:
		#	diff = 360-diff
		#if state2[i]-state1[i] > 0:
		#	state[i] = state1[i]+diff/2
		#else:
		#	state[i] = state2[i]+diff/2
		val=(state2[i]+state1[i])/2
		if diff > 180:
			state[i]=(val+180)%360
		else:
			state[i]=val%360
	return state

def edgechecker(state1,state2,epsilon):
	if dist(state1,state2) <= epsilon:
		return 1
	else:
		mid = midpt(state1,state2)
		if checkobs(obstacles,mid,dof):
			return 0
		else:
			return (edgechecker(state1,mid,epsilon) and edgechecker(mid,state2,epsilon))
G = nx.Graph()
def localplanner(indlist):
	guiltyedges=0
	vert = indlist[0]
	distances[vert][0]=sys.maxint
	for i in range(1,len(indlist)):
		j = indlist[i]
		if edgechecker(theta[vert],theta[j],dist(theta[vert],theta[j])/50)==0:
			distances[vert][i] = sys.maxint
			guiltyedges=guiltyedges+1
		else:
			G.add_edge(vert,j,distance=distances[vert][i]);

def planner():
	for k in range(0,space):
		localplanner(indices[k])

planner()
i=5
soln = nx.dijkstra_path(G,0,i,'distance')
print soln
len1=len(soln)


if len1>0:
	final = [[0]*2 for x in range((dof+1)*len1)]
	for i in range(0,len1):
		result=makeconfig(theta[soln[i]],dof)
		for j in range(0,dof+1):
			final[j+(dof+1)*i] = [str(ab) for ab in result[j]];final[j+(dof+1)*i] = " ".join(final[j+(dof+1)*i])


final = "\n".join(final)
fp = open("final.txt","w")
fp.write(final)


with open("final.txt") as f:
    data = f.read()

data = data.split('\n')
del data[-1]
x = [[0]*3 for p in range(len1)]
y = [[0]*3 for p in range(len1)]
for i in range(0,len1):
	x[i] = [row.split(' ')[0] for row in data[3*i:3*i+3] ]
	y[i] = [row.split(' ')[1] for row in data[3*i:3*i+3] ]

fig = plt.figure()
pylab.ylim([-500,500])
pylab.xlim([-500,500])
ax1 = fig.add_subplot(111)

ax1.set_title("Plot title...")    
a = [0]*2;b=[0]*2
for i in range(0,numobs):
	a[0]=obstacles[i].p1.x
	a[1]=obstacles[i].p2.x
	b[0]=obstacles[i].p1.y
	b[1]=obstacles[i].p2.y
	ax1.plot(a,b, c='k', linewidth=8)
name = "demo"+str(0)+".png"
ax1.plot(x[0],y[0], c='r')
pylab.savefig(name)
for i in range(1,len1-1):
	name = "demo"+str(i)+".png"
	ax1.plot(x[i],y[i], c='y')
	pylab.savefig(name)
name = "demo"+str(0)+".png"
ax1.plot(x[i],y[i], c='b')
pylab.savefig(name)
leg = ax1.legend()

plt.show()