import math

from analyzer.epoint import EPoint

class Line:

	def __init__(self,alpha,d):

		self._alpha=alpha

		self._d=d

		self._sin=math.sin(alpha)

		self._cos=math.cos(alpha)

	@staticmethod

	def fromNormal(a,b,c):

		"""ax + by + c = 0"""

		sign=-c/abs(c) if c!=0 else 1

		norm=sign*math.sqrt(a**2+b**2)

		(a_,b_,c_)=(a/norm,b/norm,c/norm)

		alpha=math.acos(a_) if b_>=0 else 2*math.pi-math.acos(a_)

		return Line(alpha,-c_)

	@staticmethod

	def fromPoints(a,b):

		return Line.fromNormal(a.y-b.y, b.x-a.x, (b.y-a.y)*a.x+(a.x-b.x)*a.y)

	def toNormal(self):

		# https://en.wikipedia.org/wiki/Line_(mathematics)#In_normal_form

		"""ax + by + c = 0"""

		return (self._cos, self._sin, -self._d)

	def intersect(self,line):

		if self._alpha==line._alpha: return None

		(a,b,c)=self.toNormal()

		(d,e,f)=line.toNormal()

		x=(b*f-c*e)/(a*e-b*d)

		y=(c*d-a*f)/(a*e-b*d)

		return EPoint(x,y)

	def distanceTo(self,point):

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

		(a,b,c)=self.toNormal()

		return abs(a*point.x+b*point.y+c) # a**2 + b**2 == 1 for Hesse normal form

def angleDiff(alpha,beta):

	diff=abs(alpha-beta)

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

	return diff

import sys

sys.path.append("../src")

import math

from datetime import datetime

import logging as log

import numpy as np

import scipy.optimize

import scipy.signal

import cv2 as cv

from geometry import EPoint,Line

DEBUG=True

class LineBag:

	def __init__(self):

		self._lines=[]

	def put(self,score,alpha,beta,peaks):

		self._lines.append((score,alpha,beta,peaks))

	def pull(self,count):

		self._lines.sort(reverse=True)

		res=[]

		for (score,alpha,beta,peaks) in self._lines:

			if any(abs(alpha-gamma)<10 and abs(beta-delta)<10 for (_,gamma,delta,_) in res): continue

			if any((beta-delta)!=0 and (alpha-gamma)/(beta-delta)<0 for (_,gamma,delta,_) in res): continue

			res.append((score,alpha,beta,peaks))

			if len(res)>=count: break

		return res

class HoughTransform:

	def __init__(self,img):

		self._angleBandwidth=30 # degrees

		(h,w)=img.shape[:2]

		self._diagLen=int(np.sqrt(h**2+w**2))+1

		self._center=(w//2,h//2)

		self._acc=np.zeros((180,self._diagLen),dtype=np.int32)

		self.update(img)

	def extract(self):

		img=self._createImg()

		self.show(img)

		lines=self._detectLines()

		res=[]

		i=0

		for (score,alpha,beta,peaks) in lines:

			log.debug("score: %s",score)

			log.debug("alpha, beta: %s, %s",alpha,beta)

			self._drawLine(img,alpha,beta,peaks,i)

			keys=self._readLineKeys(alpha,beta)

			for k in peaks:

				(alphaDeg,d)=keys[k]

			i+=1

		self.show(img)

		return res

	def update(self,img,weight=1):

		start=datetime.now().timestamp()

		for (r,row) in enumerate(img):

			for (c,pix) in enumerate(row):

				if pix==0: continue

				for alphaDeg in range(0,180):

					d=self._computeDist(c,r,alphaDeg)+self._diagLen//2

					self._acc[(alphaDeg,d)]+=weight

		log.debug("Hough updated in %s s",round(datetime.now().timestamp()-start,3))

	def _computeDist(self,x,y,alphaDeg):

		alphaRad=alphaDeg*math.pi/180

		(x0,y0)=self._center

		d=dx*math.cos(alphaRad)+dy*math.sin(alphaRad)

		return round(d)

	def _detectLines(self):

		bag=LineBag()

				accLine=[self._acc[key] for key in self._readLineKeys(alpha,beta)]

				(peaks,props)=scipy.signal.find_peaks(accLine,prominence=0)

				(prominences,peaks)=zip(*sorted(zip(props["prominences"],peaks),reverse=True)[:19])

				bag.put(sum(prominences),alpha,beta,peaks)

		return bag.pull(2)

	def _readLineKeys(self,alpha,beta):

		n=self._diagLen-1

		res=[]

		for i in range(n+1):

			k=round((alpha*(n-i)+beta*i)/n)

				k=k%180

			res.append((k,i))

		return res

	def _createImg(self):

		maxVal=self._acc.max()

		arr=np.expand_dims(np.uint8(255*self._acc//maxVal),axis=2)

		img=np.concatenate((arr,arr,arr),axis=2)

		(h,w)=img.shape[:2]

		for x in range(0,w,4): # y axis

			img[h//2,x]=[255,255,255]

		for y in range(0,h,4):

			img[y,w//2]=[255,255,255]

		return img

	def _drawLine(self,img,alpha,beta,peaks,colorKey):

		colors=[[0,255,255],[255,0,255],[255,255,0]]

		color=colors[colorKey]

		(h,w)=img.shape[:2]

		keys=self._readLineKeys(alpha,beta)

		for (y,x) in keys:

			if x%3!=0: continue

			if y<0 or y>=h: continue

			img[y,x]=color

		for k in peaks:

			(y,x)=keys[k]

			cv.drawMarker(img,(x,y),color,cv.MARKER_TILTED_CROSS,8)

def show(img,filename="x"):

	cv.imshow(filename,img)

	cv.waitKey(0)

	cv.destroyAllWindows()

def filterVert(edges):

	kernel = np.array([[1,0,1],[1,0,1],[1,0,1]],np.uint8)

	edges = cv.erode(edges,kernel)

	kernel=np.array([[0,1,0],[0,1,0],[0,1,0]],np.uint8)

	edges=cv.dilate(edges,kernel)

	return edges

def filterHor(edges):

	kernel = np.array([[1,1,1],[0,0,0],[1,1,1]],np.uint8)

	edges = cv.erode(edges,kernel)

	kernel=np.array([[0,0,0],[1,1,1],[0,0,0]],np.uint8)

	edges=cv.dilate(edges,kernel)

	return edges

def filterDiag(edges):

	kernel = np.array([[0,0,1],[1,0,0],[0,1,0]],np.uint8)

	edges1 = cv.erode(edges,kernel)

	kernel=np.array([[1,0,0],[0,1,0],[0,0,1]],np.uint8)

	edges1=cv.dilate(edges1,kernel)

	kernel = np.array([[0,1,0],[1,0,0],[0,0,1]],np.uint8)

	edges2 = cv.erode(edges,kernel)

	kernel=np.array([[0,0,1],[0,1,0],[1,0,0]],np.uint8)

	edges2=cv.dilate(edges2,kernel)

	return edges1+edges2

def prepareEdgeImg(img):

	gray=cv.cvtColor(img,cv.COLOR_BGR2GRAY)

	show(gray,"greyscale image")

	edges=cv.Canny(gray,70,130)

	show(edges,"Canny edge detector")

	edges=filterHor(edges)+filterVert(edges)+filterDiag(edges)

	show(edges,"kernel filtered edges")

	return edges

import math

import random

from unittest import TestCase

from geometry import Line,EPoint

random.seed(361)

class TestLine(TestCase):

	def testFromNormal(self):

		p=Line.fromNormal(1,0,-1) # x-1=0

		self.assertEqual(p._alpha,0)

		self.assertEqual(p._d,1)

		q=Line.fromNormal(1,1,-2) # x+y-2=0

		self.assertAlmostEqual(q._alpha,math.pi/4)

		self.assertAlmostEqual(q._d,math.sqrt(2))

		r=Line.fromNormal(0,1,1) # y+1=0

		self.assertAlmostEqual(r._alpha,math.pi*3/2)

		self.assertEqual(r._d,1)

	def testFromPoints(self):

		ab=Line.fromPoints(EPoint(1,3),EPoint(1,-1))

		self.assertEqual(ab._alpha,0)

		self.assertEqual(ab._d,1)

		cd=Line.fromPoints(EPoint(0,2),EPoint(-1,3))

		self.assertAlmostEqual(cd._alpha,math.pi/4)

		self.assertAlmostEqual(cd._d,math.sqrt(2))

		ef=Line.fromPoints(EPoint(-2,-1),EPoint(-4,-1))

		self.assertAlmostEqual(ef._alpha,math.pi*3/2)

		self.assertEqual(ef._d,1)

	def testIntersect(self):

		for i in range(10):

			a=EPoint(random.randint(-100,100),random.randint(-100,100))

			b=EPoint(random.randint(-100,100),random.randint(-100,100))

			c=EPoint(random.randint(-100,100),random.randint(-100,100))

			ab=Line.fromPoints(a,b)

			ac=Line.fromPoints(a,c)

			a_=ab.intersect(ac)

			self.assertAlmostEqual(a.x,a_.x)

			self.assertAlmostEqual(a.y,a_.y)

	def testDistanceTo(self):

		p=Line(0,1)

		q=Line(math.pi/4,math.sqrt(2))

		r=Line(math.pi*3/2,1)

		a=EPoint(0,0)

		b=EPoint(1,0)

		c=EPoint(-1,-1)

		self.assertAlmostEqual(p.distanceTo(a),1)

		self.assertAlmostEqual(p.distanceTo(b),0)

		self.assertAlmostEqual(p.distanceTo(c),2)

		self.assertAlmostEqual(q.distanceTo(a),math.sqrt(2))

		self.assertAlmostEqual(q.distanceTo(b),math.sqrt(2)/2)

		self.assertAlmostEqual(q.distanceTo(c),2*math.sqrt(2))

		self.assertAlmostEqual(r.distanceTo(a),1)

		self.assertAlmostEqual(r.distanceTo(b),1)

		self.assertAlmostEqual(r.distanceTo(c),0)