import cv2
import numpy as np
from time import sleep
from PiRapidStream import PiRapidStream
##def create_sin_table(x):
##    return np.sin(x*np.pi/180.0)
##def create_cos_table(x):
##    return np.cos(x*np.pi/180.0)
##def arctable(x):
##    dx,dy=x[0],x[1]
##    return np.uint8(np.remainder(np.arctan2(dy,dx) * 180 / np.pi+180,180))
accu=[]
accumulator=np.zeros((1200,181),dtype=np.uint8)
def rho(x,y,theta):
    return np.abs(np.rint(x*cos_table[theta]+y*sin_table[theta])).astype(np.int)

def n_max(a, n):
    '''
    Return the N max elements and indices in a
    '''
    indices = a.ravel().argsort()[-n:]
    indices = (np.unravel_index(i, a.shape) for i in indices)
    return [(a[i], i) for i in indices]

def updateac():
    global accumulator
    for (i,j) in np.random.randint(480,size=(10000,2)):
        if accu[i][j]:
            grad=gradient[i,j]
            remain=np.remainder(grad-5,180)
            rang=angles[remain:remain+10:1]
            for theta in rang:
                r=rho(j,i,theta)
                accumulator[r,theta]+=1
            

            
#angles=[np.remainder(i,180) for i in xrange(0,361)]
#sin_table=map(create_sin_table,angles)
#cos_table=map(create_cos_table,angles)
#arcmesh=np.array(np.meshgrid(np.arange(-255,256),np.arange(-255,256))).T.reshape(-1,2)
#arcs=np.uint8(map(arctable,arcmesh))
#np.savez_compressed('tables.npz',arcs=arcs,sin_table=sin_table,cos_table=cos_table,angles=angles)
angles=np.load('tables.npz')['angles']
sin_table=np.load('tables.npz')['sin_table']
cos_table=np.load('tables.npz')['cos_table']
arctan_table=np.load('tables.npz')['arcs'].reshape(511,511)




   
##src=cv2.imread('/home/pi/Desktop/Raspberry/opencv/fooo.jpg')
##src=cv2.cvtColor(src, cv2.COLOR_BGR2GRAY)
vs = PiRapidStream(resolution=(480,480),fps=20,form='yuv',exp=30000,gain=1.15)
vs.startcapt()

while cv2.waitKey(1) != ord("q"):
    #accumulator=np.zeros((1200,181),dtype=np.uint8)
    accumulator=np.zeros((480,480),dtype=np.uint8)
    src=vs.frame
    retval, src = cv2.threshold(src, 210, 255, cv2.THRESH_BINARY)
    cv2.imshow('so',src)

##    dx = cv2.Sobel(src, cv2.CV_16S,1,0, ksize=1)
##    dy = cv2.Sobel(src, cv2.CV_16S,0,1, ksize=1)
##    gradient = arctan_table[dx+255,dy+255]
##    dx=np.uint8(np.absolute(dx))
##    dy=np.uint8(np.absolute(dy))
##    accu=cv2.addWeighted(dx,0.5,dy,0.5,0,-1)
    #th,accu=cv2.threshold(accu,250,0,cv2.THRESH_BINARY)

    lines = cv2.HoughLines(src, 1, np.pi/90.0, 50)
    if lines is not None:
        for line in lines:
            rho = line[0][0]
            theta = line[0][1]
            a = np.cos(theta)
            b = np.sin(theta)
            x0, y0 = a*rho, b*rho
            pt1 = ( int(x0+1000*(-b)), int(y0+1000*(a)) )
            pt2 = ( int(x0-1000*(-b)), int(y0-1000*(a)) )
            cv2.line(accumulator, pt1, pt2, (0, 0, 255), 3)

    cv2.imshow('s',accumulator)
    #cv2.imshow('g',np.uint8(gradient))
    #updateac()
    #cv2.imshow('fickdich',accumulator)
    if cv2.waitKey(1) == ord("a"):
        print(n_max(accumulator,100))


cv2.destroyAllWindows()
vs.stopcapt()
print "quit"    


