# Este script ejecuta la funcion que permite detectar el nuevo fiducial

import cv 			# se importan los modulos necesarios
import cv2
import numpy as np

aux=cv2.imread("fid",1)             
fid=cv2.resize(aux,(150,150))       
fid=cv2.flip(fid,1)              
fiducial=cv2.cvtColor(fid,cv2.COLOR_BGR2GRAY)
M=np.float32([[0,0,0],[0,0,0]])     
Mrot=cv2.getRotationMatrix2D((150/2,150/2),90,1)
kernel=np.ones((3,3),np.uint8)
kernel2=np.ones((7,7),np.uint8)
k2=np.ones((3,3),np.uint8)
criterio=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001)

# -----------------------------------------------------------------------------------------------
# detectarPatron(img,fiducial,kernel,Mrot,M) -> found,pts1
# Parametros: 	
#	img: Es la imagen a analizar en formato BGR.
#	u1: Umbral que se aplicara durante la busqueda de contornos 
#	u2: Umbral que se aplicara durante la comparacion
#   zoom: Escala en que se ampliara la imagen durante la busqueda.
# Retorno:
#	found: Vale 1 si se encontro el fiducial en img.
#	pts1: contiene las coordenadas de las 4 esquinas del fiducial si found=1.
#	u1: Valor estimado de u1 que permitira mejorar la busqueda de contornos
#	u2: Valor estimado de u2 que permitira mejorar la comparacion
# ----------------------------------------------------------------------------------------------

def detectarPatron(img,u1,u2,zoom): 	
  	found=0

	rows,cols,ch = img.shape 				# se leen las dimensiones de la imagen		
	gris=cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)		# se la pasa a escala de grises
								# se aplica la ampliacion indicada por el zoom
	gris=cv2.resize(gris[int(rows/2-rows/(2*zoom)):int(rows/2+rows/(2*zoom)),int(cols/2-cols/(2*zoom)):int(cols/2+cols/(2*zoom))],(cols,rows))

	#gris=cv2.equalizeHist(gris)				# se la ecualiza 
	pts=np.float32([[0,0],[150,0],[150,150],[0,150]])	# se declaran vectores auxiliares
	pts1=np.zeros((10,2),np.float32)
	pts2=np.float32([[0,0],[150,0],[150,150],[0,150]])
								# se obtiene una imagen binaria mediante un umbral variable
	u1,thres=cv2.threshold(gris,u1,255,cv2.THRESH_OTSU+cv2.THRESH_BINARY);
	#thres=cv2.adaptiveThreshold(gris,255,cv2.ADAPTIVE_THRESH_MEAN_C,cv2.THRESH_BINARY_INV,11,1)   
	thres=cv2.morphologyEx(thres,cv2.MORPH_OPEN,kernel)	# se elimina el ruido que resulta de la funcion anterior
	thres=cv2.bitwise_not(thres)
	#thres=cv2.dilate(thres,kernel2,iterations=1)
	#thres=cv2.erode(thres,kernel2,iteratimg=cv2.resize(img[120:200,90:150],(320,240))ions=1) 
	#cv2.imshow("binaria",thres)
								# se buscan los contornos y se los almacena en un vector "contorno"  
	contorno,hie=cv2.findContours(thres,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)
	#cv2.drawContours(img,contorno,-1,(0,0,255),2)
	for nc in range(len(contorno)):					# para cada uno de los contornos encontrados
		epsilon=0.1*cv2.arcLength(contorno[nc],True)		# se define el nivel de error, segun su perimetro
		#for p in cnt:
		#     cv2.circle(img,tuple(p.ravel()),2,(0,0,255),-1)
		approx=cv2.approxPolyDP(contorno[nc],epsilon,True)	# y se lo aproxima con un poligono
		area=cv2.contourArea(contorno[nc])			# se calcula el area del contorno
		found=0 					# inicializamos found para indicar que no fue encontrado	
		if len(approx)==4 and area>500:			# si el contorno tiene 4 esquinas y el area supera el umbral:
			#print area
			pts[0]=approx[0]			# se almacenan las 4 esquinas
			pts[1]=approx[1]
			pts[2]=approx[2]
			pts[3]=approx[3]
			#for p in pts:
			#     cv2.circle(img,tuple(p.ravel()),2,(0,0,255),-1) 
			M=cv2.getPerspectiveTransform(pts,pts2)	# se calcula la transformacion perspectiva que mapea los puntos de la imagen en un cuadrado
			Minv=cv2.getPerspectiveTransform(pts2,pts)		# para corregir la proyeccion del fiducial y facilitar la comparacion
			#print M
			dst=cv2.warpPerspective(gris,M,(cols,rows)) # se aplica la transformacion a la imagen
			pat=dst[0:150,0:150]			# se recorta la imagen, dejando solo la region que nos es util
			u2,pat=cv2.threshold(pat,u2,255,cv2.THRESH_OTSU+cv2.THRESH_BINARY);
			#cv2.imshow("pat",pat)
			rot=0  					# inicializamos la cantidad de rotaciones efectuadas
			while rot<4: 				# mientras se hayan hecho menos de 4 rotaciones
				result=cv2.bitwise_xor(pat,fiducial)	# se aplica un XOR bit a bit entre la imagen y el fiducial
				#cv2.imshow("desproyectado",pat)		# se muestran los resultados
				#cv2.imshow("original",fiducial)
				#cv2.imshow("xor",result)
				suma=cv2.integral(result)		# se realiza la sumatoria de todos los pixeles 
				#print suma[150,150]
				if suma[150,150]<1000000:               # si la suma es inferior al umbral, la region de la imagen corresponde al fiducial	
					#cv2.drawContours(img,contorno[nc],-1,(0,0,255),2)
					Ncont_int=hie[0,nc,2]
					#cv2.drawContours(img,contorno[Ncont_int],-1,(0,255,255),2)
					while Ncont_int!=-1:
						eps2=0.025*cv2.arcLength(contorno[Ncont_int],True)
						ap=cv2.approxPolyDP(contorno[Ncont_int],eps2,True)
						#cv2.drawContours(img,contorno[Ncont_int],-1,(0,255,255),2)
						#print len(ap)
						if len(ap)==6:
							found=1
						Ncont_int=hie[0,Ncont_int,0]
			        	if found==1:
							break                           # y se sale del bucle
				if found==1:
					break                
	                  	# si no se dio el nivel de coincidencia:
				pat=cv2.warpAffine(pat,Mrot,(150,150)) # se vuelve a probar rotando 90 grados la imagen 
				rot=rot+1 				# y se incrementa la cantidad de rotaciones efectuadas
			       
		if found==1: 				# Si el contorno corresponde al fiducial, se ordenan las esquinas segun la cantidad de rotaciones
							# que fueron necesarias para encontrarlo
			if rot==0:			# Si no hubo rotaciones
				pts1[0]=pts[0]
				pts1[1]=pts[1]
				pts1[2]=pts[2]
				pts1[3]=pts[3]
			elif rot==1:			# Si hubo 1 rotacion
				pts1[0]=pts[1]
				pts1[1]=pts[2]
				pts1[2]=pts[3]
				pts1[3]=pts[0]
			elif rot==2:			# Si hubo 2 rotaciones
			 	pts1[0]=pts[2]
			   	pts1[1]=pts[3]
			   	pts1[2]=pts[0]
				pts1[3]=pts[1]
			elif rot==3:			# Si hubo 3 rotaciones
				pts1[0]=pts[3]
				pts1[1]=pts[0]
				pts1[2]=pts[1]
				pts1[3]=pts[2]
 
						        # se corrije la posicion de los puntos segun el zoom usado
			mod=np.zeros((6),np.float32)			
			for k in range(len(mod)):
				mod[k]=np.sqrt((ap[k,0,0]-pts1[3,0])**2+(ap[k,0,1]-pts1[3,1])**2)				
			val_min=mod[0]
			ind_min=0
			for k in range(len(mod)):
				if mod[k]<val_min:
					ind_min=k
					val_min=mod[k]			
			ap=np.concatenate((ap[ind_min:],ap[:ind_min]),0)
			if(len(ap[:,0,:])==9):
				found=0
				break
			pts1[4:]=ap[:,0,:]						
			
			dy=rows/2-rows/(2*zoom)
			dx=cols/2-cols/(2*zoom)
			for i in range(4):
				pts1[i,0]=pts1[i,0]/zoom+dx
				pts1[i,1]=pts1[i,1]/zoom+dy
			cv2.cornerSubPix(gris,pts1,(5,5),(-1,-1),criterio)
			break
	
	return found,pts1,u1,u2 			# Se retornan los resultados



