import itertools

from analyzer.epoint import EPoint,homogenize

from analyzer.grid import transformPoint

class Line():

	def __init__(self,a,b):

		self.a=a

		self.b=b

		self.points={a,b}

	def getSortedPoints(self):

		return tuple(sorted(self.points))

	def computeAngle(self,line):

		ab=self.a-self.b

		cd=line.a-line.b

		alpha=math.atan(ab.y/ab.x)

		gamma=math.atan(cd.y/cd.x)

		fi=max(alpha,gamma)-min(alpha,gamma)

		return min(fi,math.pi-fi)

	def intersect(self,line):

		p=self.toProjective()

		q=line.toProjective()

		return EPoint.fromProjective(np.cross(p,q))

	def toProjective(self):

		return homogenize(np.cross(self.a.toProjective(),self.b.toProjective()))

	def transform(self,matrix):

		a=EPoint.fromProjective(transformPoint(self.a.toProjective(),matrix))

		b=EPoint.fromProjective(transformPoint(self.b.toProjective(),matrix))

		if a is None or b is None: return None

		return Line(a,b)

	def __str__(self): return "({0},{1})".format(self.a,self.b)

	def __repr__(self): return "Line({0},{1})".format(repr(self.a),repr(self.b))

class PolarHough:

	# http://www.cis.pku.edu.cn/vision/vpdetection/LiPYZ10isvc.pdf

	def __init__(self,anglePrecision,lengthPrecision):

		self._anglePrecision=anglePrecision

		self._lengthPrecision=lengthPrecision

		self._maxLength=4096

		n=math.ceil(2*math.pi/anglePrecision)

		self._acc=[[] for i in range(n)]

	def put(self,item):

		k=int(item[0]//self._anglePrecision)

		self._acc[k].append(item)

	def extract(self,count):

		vanishingPoints=[]

		angles=self._extractAngles(count)

		angles=[alpha for (alpha,prominence) in angles]

		bins=self._mapAngles(angles)

		for (alpha,bin) in zip(angles,bins):

			(length,sampleCount)=self._extractLength([dist for (beta,dist) in bin])

			vanishingPoints.append((alpha,length))

		return vanishingPoints

	def _extractAngles(self,k):

		lens=np.array(list(map(len,self._acc)))

		print(lens)

		(peakKeys,info)=scipy.signal.find_peaks(lens,prominence=0)

		res=sorted(zip(info["prominences"],peakKeys),reverse=True)[:k]

		res=[(key*self._anglePrecision,prominence) for (prominence,key) in res]

		print("(angle, prominence):",res,"...",[alpha/self._anglePrecision for (alpha,_) in res])

		return res

	def _mapAngles(self,angles):

		res=[[] for alpha in angles]

		for (i,bin) in enumerate(self._acc):

			beta=i*self._anglePrecision

			key=min(zip(map(lambda alpha: angleDiff(alpha,beta), angles), itertools.count()))[1]

			res[key].extend(bin)

		return res

	def _extractLength(self,arr):

		acc=np.zeros(self._maxLength+1,dtype=np.int32)

		for dist in arr:

			dist=min(dist,self._maxLength)

			acc[int(dist//self._lengthPrecision)]+=1

		res=acc.argmax()

		print("(length, count):",(res*self._lengthPrecision,acc[res]))

		return (res*self._lengthPrecision,acc[res])

def angleDiff(alpha,beta):

	diff=abs(alpha-beta)

	if diff>math.pi: diff=2*math.pi-diff

	return diff

def point2lineDistance(a,b,p):

	# https://en.wikipedia.org/wiki/Point-line_distance#Line_defined_by_two_points

	ab=b-a

	num=abs(ab.y*p.x - ab.x*p.y + b.x*a.y - a.x*b.y)

	denum=math.sqrt(ab.y**2+ab.x**2)

	return num/denum # double_area / side_length == height

def computeRectiMatrix(p,q,r,s):

	# p || q, r || s

	vanish1=homogenize(np.cross(p.toProjective(),q.toProjective()))

	vanish2=homogenize(np.cross(r.toProjective(),s.toProjective()))

	horizon=homogenize(np.cross(vanish1,vanish2))

	return np.matrix([horizon,[0,1,0],[0,0,1]])

def scoreMatrix(matrix,p,r,lines):

	p_=p.transform(matrix)

	r_=r.transform(matrix)

	if p_ is None or r_ is None:

		return math.inf

	score=0

	for ab in lines:

		if ab is p or ab is r: continue

		ab_=ab.transform(matrix)

		if ab_ is None:

			score+=math.pi/2

			continue

		alpha=min(ab_.computeAngle(p_), ab_.computeAngle(r_))

		if alpha<=math.pi/30:

			score+=alpha

		else: score+=math.pi/2

	return score

def groupParallels(lines,tolerance,w):

	ctrl=False

	for (i,p) in enumerate(lines):

		for (j,q) in enumerate(lines[i+1:]):

			a=p.intersect(q)

			if a.y>0 and a.x<w: continue

			for r in lines[i+1+j+1:]:

				b=r.intersect(p) # !! ideal points

				c=r.intersect(q)

				ab=b-a

				ac=c-a

				if abs(ab.x*ac.y-ab.y*ac.x)/2<=tolerance: # area

					yield (p,q,r)

					ctrl=True

					break

			if ctrl: break

		if ctrl:

			ctrl=False

			continue