import os
import cv2
import sys
import math
import time
import json
import argparse
import numpy as np
from ownLibraries.pathsandnames import PathsAndNames

debug = False
nuevaLocalizacion = ''
# We get the current working git branch
gitBranches = os.popen('git branch').read().replace(' ','').split('\n')
gitVersion = [branch for branch in gitBranches if '*' in branch][0]

def convertirAJSON(lista = None,file = './datos.npy'):
	if lista == None:
		lista = np.load(file)

	diccionarioAJson = {'lowResolution':{	'trafficLightPixels':lista[0],
										'departure':lista[1],
										'arrival':lista[2],
										'right':lista[3],
										'left':lista[4],
										'angle':lista[5][0],
										'regionOfInterest':[]
										},
					'highResolution':{ 'trafficLightPixels':lista[7],
										'departure':[],
										'arrival':[],
										'right':[],
										'left':[],
										'angle':0,
										'regionOfInterest':lista[6]
										},
					'location':nuevaLocalizacion,
					'gpsCoordinates':{'lat':0,'lon':0,'hei':0},
					'debug':debug,
					'gitVersion':gitVersion,
					'lucam':5,
					'notes':''}
	with open(jsonToWrite, 'w') as file:
		json.dump(diccionarioAJson, file)
	print('Archivo guardado como json exitosamente')

directorioDeVideos = PathsAndNames.directorioDeVideos+'/'
jsonToWrite = PathsAndNames.folderDeInstalacion+'/datos.json'

parser = argparse.ArgumentParser(description='Add newData')
parser.add_argument('-l', '--newLocation', default = None, type=str, help="Add new subfolder")
parser.add_argument('-s', '--source', default = None, type=str, help="Add input for debug")
parser.add_argument('-c', '--convertir', default = None, type=str, help="justConvertFile")
args = parser.parse_args()

if not os.path.exists(PathsAndNames.folderDeInstalacion):
	os.makedirs(PathsAndNames.folderDeInstalacion)

if args.newLocation != None:
	nuevaLocalizacion = args.newLocation
	print('Trabajando para '+nuevaLocalizacion)
else:
	nuevaLocalizacion = 'debug'
	print('Trabajando en modo debug')

if args.source != None:
	nameSourceVideo = args.source
	jsonToWrite = jsonToWrite[:-10]+nameSourceVideo[:-3]+'json'
	archivo = args.source
else:
	nameSourceVideo = 0
	archivo = 'datos.mp4'


help_message= '''Selección zona de análisis
Incluye la seleccion de semaforo en imagen de Alta resolución
'''
listaSemFinal=[]
listaCorte=[]
listaSem=[]
lista=[]
listaAux=[]
listaAux1=[]
listaAux2=[]
listaAux3=[]
listaSemaAlta=[]
listCorteAltaRes=[]

###############Get indices#################
def obtenerIndicesSemaforo(poligono640):
	punto0 = poligono640[0]
	punto1 = poligono640[1]
	punto2 = poligono640[2]
	punto3 = poligono640[3]

	vectorHorizontal = punto3 - punto0
	vectorVertical = punto1 - punto0
	pasoHorizontal = vectorHorizontal/8
	pasoVertical = vectorVertical/24

	indices = []


	for j in range(24):
		for i in range(8):
			indices.append((punto0+i*pasoHorizontal+j*pasoVertical).tolist())
	#print('len of indices', len(indices))
	#print('single index', indices[0])
	#indices = [[round(x[0]),round(x[1])] for x in indices]
	return indices

###########Calculate new Points############
def transformIma(lista):
	x2=lista[0][0]
	y2=lista[0][1]
	x3=lista[1][0]
	y3=lista[1][1]
	distancia=math.sqrt((x2-x3)**2+(y2-y3)**2)
	altura=distancia/3
	if y2>y3:
		#print('caso1')
		anguloInicial=math.asin((y2-y3)/distancia)
		anguloInicialGrados=anguloInicial*180/(math.pi)
		anguloGrados=180-90-anguloInicialGrados
		anguloRadians=anguloGrados*(math.pi)/180
		y=altura*math.sin(anguloRadians)
		x=altura*math.cos(anguloRadians)
		x1=int(x2-x)
		y1_0=int(y2-y)
		y1=int(y1_0-altura)
		x4=int(x3-x)
		y4_0=int(y3-y)
		y4=int(y4_0-altura)
		poligon=[(x1,y1_0),(x2,y2),(x3,y3),(x4,y4_0)]
		poligonAdd=[(x1,y1),(x2,y2),(x3,y3),(x4,y4)]
	if y3==y2:
		#print('caso2')
		x1=x2
		y1_0=int(altura)
		y1=int(2*altura)
		x4=x3
		y4_0=int(altura)
		y4=int(2*altura)
		poligon=[(x1,y1_0),(x2,y2),(x3,y3),(x4,y4_0)]
		poligonAdd=[(x1,y1),(x2,y2),(x3,y3),(x4,y4)]
	if y3>y2:
		#print('caso3')
		anguloInicial=math.asin((y3-y2)/distancia)
		anguloInicialGrados=anguloInicial*180/(math.pi)
		anguloGrados=180-90-anguloInicialGrados
		anguloRadians=anguloGrados*(math.pi)/180
		y=altura*math.sin(anguloRadians)
		x=altura*math.cos(anguloRadians)
		x1=int(x2+x)
		y1_0=int(y2-y)
		y1=int(y1_0-altura)
		x4=int(x3+x)
		y4_0=int(y3-y)
		y4=int(y4_0-altura)
		poligon=[(x1,y1_0),(x2,y2),(x3,y3),(x4,y4_0)]
		poligonAdd=[(x1,y1),(x2,y2),(x3,y3),(x4,y4)]
	return poligon, poligonAdd
###### Get points with the mouse click######
def get_PointsSemaforZona(event,x,y,flags,param):
	global frame
	if event == cv2.EVENT_LBUTTONDOWN:
		listaCorte.append((x,y))
		if len(listaCorte)!= 0:
			cv2.circle(frame, (x,y),2,(0,255,0),-1)
			cv2.imshow('semaforo_Zona',frame)
		if len(listaCorte)== 2:
			frame=cv2.rectangle(frame,listaCorte[0],listaCorte[1],(0,0,255),3)
			cv2.imshow('semaforo_Zona',frame)
########## Traffic light Selection##########
def get_PointsSema(event,x,y,flags,param):
	global imag
	if event == cv2.EVENT_LBUTTONDOWN:
		listaSem.append((x,y))
		if len(listaSem)!= 0:
			cv2.circle(imag, (x,y),2,(0,0,255),-1)
			cv2.imshow('semaforo',imag)
##########################################
def get
ent,x,y,flags,param):
	global frame
	if event == cv2.EVENT_LBUTTONDOWN:
		listaAux.append((x,y))
		if len(lista)>0:
			listaAux1.append((x//2,y//2))
		else:
			listaAux1.append((x//2,y//2))
		if len(listaAux)!= 0:
			cv2.circle(frame, (x,y),2,(0,255,255),-1)
			cv2.imshow('First_Frame',frame)
		if len(listaAux)>= 2:
			cv2.line(frame,listaAux[len(listaAux)-1],listaAux[len(listaAux)-2],(0,255,255),1)
		if len(listaAux)>= 4:
			cv2.line(frame,listaAux[len(listaAux)-1],listaAux[len(listaAux)-len(listaAux)],(0,255,255),1)
		cv2.imshow('First_Frame',frame)
############High resolution#############
def get_BigRectangle(event,x,y,flags,param):
	global frame
	if event == cv2.EVENT_LBUTTONDOWN:
		listaAux2.append((x,y))
		if len(listaAux2)<3:
			listCorteAltaRes.append((x*4,y*4))
			cv2.circle(frame, (x,y),3,(0,255,0),-1)
			cv2.imshow('FrameDeAltaResolucion',frame)
		if len(listaAux2)==2:
			lista.append((listCorteAltaRes))
			frame=cv2.rectangle(frame,listaAux2[0],listaAux2[1],(0,0,255),3)
			cv2.imshow('FrameDeAltaResolucion',frame)

def get_Big_Sem_Polig(event,x,y,flags,param):
	if event == cv2.EVENT_LBUTTONDOWN:
		global frame
		listaAux2.append((x,y))
		if len(listaAux2)!= 0:
			cv2.circle(frame, (x,y),2,(0,255,0),-1)
			cv2.imshow('FrameResolucion',frame)
		if len(listaAux2)== 2:
			frame=cv2.rectangle(frame,listaAux2[0],listaAux2[1],(0,0,255),2)
			cv2.imshow('FrameResolucion',frame)
###############################################################
def get_TrafficLight(event,x,y,flags,param):
	if event == cv2.EVENT_LBUTTONDOWN:
		listaAux3.append((x,y))
		if len(listaAux3)<5:
			listaSemaAlta.append((Const_X*(Punto_X0+const_x_zoom*x),Const_Y*(Punto_Y0+const_y_zoom*y)))
			cv2.circle(frame, (x,y),2,(0,250,255),-1)
			cv2.imshow('FrameToTrafficLight',frame)
		if len(listaAux3)==4:
			indice_Alta=obtenerIndicesSemaforo(np.array(listaSemaAlta))
			print('listaSemaforo..'+str(listaSemaAlta))
			print('Puntos obtenidos: ',indice_Alta)
			print(indice)
			lista.append((indice_Alta))
			print('Press -q- to Save and Exit')
##################################################################3####
#
def calcPoints(imag,listaS):
	imag=imag
	ancho=imag.shape[1]
	largo=imag.shape[0]
	x1=listaS[0]
	y1=listaS[1]
	x=listaCorte[0][0]+(x1*ancho)//640
	y=listaCorte[0][1]+(y1*largo)//480
	return x, y


if __name__ == '__main__':
	print (help_message)

	try:
		cap=cv2.VideoCapture(directorioDeVideos+nameSourceVideo)
		for i in range(100):
			ret, frame=cap.read()
		frame=cv2.resize(frame,(640,480))
		val=1
	except:
		val=0
	if val==0:
		print('Accediendo a imagen de flujo...')
		try:
			frame=cv2.imread(PathsAndNames.folderDeInstalacion+'/flujo.jpg')
			frame=cv2.resize(frame,(640,480))
		except:
			print('No se encontro imagen de flujo')
			print('Accediendo a cámara...')
			cap=cv2.VideoCapture(0)
			for i in range(100):
				ret, frame=cap.read()
			frame=cv2.resize(frame,(640,480))
	frame2=frame.copy()
	fram=frame.copy()
	print('select traffic light zone, press -z- to zoom ')
	cv2.namedWindow('semaforo_Zona')
	cv2.setMouseCallback('semaforo_Zona', get_PointsSemaforZona)
	while True:

		cv2 .imshow('semaforo_Zona',frame)
		keyPress = cv2.waitKey()
		if keyPress == ord('z'):
			break
		if keyPress == ord('q'):
			print ('Interrumpido...')
			break
	cv2.destroyAllWindows()

######################################
###Selección Semáforo:
	imag1=frame2[listaCorte[0][1]:listaCorte[1][1],listaCorte[0][0]:listaCorte[1][0]]
	imag=imag1.copy()
	imag=cv2.resize(imag,(640,480))
	cv2.namedWindow('semaforo')
	cv2.setMouseCallback('semaforo', get_PointsSema)
	print('Select points and press -q- to accept the traffic light points and exit')
	while(1):
		cv2.imshow('semaforo',imag)
		keyPress = cv2.waitKey()
		if keyPress&0xFF==ord('q'):
			#print('listaSemaforo..'+str(listaSem))
			rang=len(listaSem)
##TRansformando a coordenadas origen
			for i in range(0,len(listaSem)):
				x,y=calcPoints(imag1,listaSem[i])
				listaSemFinal.append((x,y))

####-----------------------------------------
			indice=obtenerIndicesSemaforo(np.array(listaSemFinal))
			print('listaSemaforo..'+str(listaSemFinal))
			print(indice)
			lista.append((indice))
			break

	cv2.destroyAllWindows()

	frame=fram.copy()
	overlay=frame.copy()
	cv2.namedWindow('First_Frame')
	cv2.setMouseCallback('First_Frame', get_Points)

	while True:
		print('press ESC to not accept and -y- to accept')
		cv2.imshow('First_Frame',frame)
		keyPress = cv2.waitKey()
		if keyPress == ord('y'):
			####listaAux1=listaAux(en magnitud)
			####listaAux1:contine valores para 320x240  ;listaAux:Valores para 640x480
			if len(listaAux)==2:
				pol1,polAdd=transformIma(listaAux)
				pol320,polAdd320=transformIma(listaAux1)
				lista.append((polAdd320))
				vrx=np.array([[pol1]],np.int32)
				pts=vrx.reshape((-1,1,2))
				cv2.polylines(frame,[pts],True,(255,0,0))
				#print(pol1)
				########################
				vrx=np.array([[polAdd]],np.int32)
				pts=vrx.reshape((-1,1,2))
				cv2.polylines(frame,[pts],True,(0,0,255))
				#print(polAdd)
				listaAux=[]
				listaAux1=[]
				cv2.imshow('First_Frame',frame)

			if len(listaAux)>2:
				print('_____Data accept..')
				print('*Select two points next press -a- to obtain Angle')
				lista.append((listaAux1))
				vrx=np.array([[listaAux]],np.int32)
				pts=vrx.reshape((-1,1,2))
				cv2.polylines(frame,[pts],True,(0,0,255))
				cv2.imshow('First_Frame',frame)
				#print('lista: '+str(lista))
				listaAux=[]
				listaAux1=[]
				#print('ListaAux Removed...'+ str(listaAux))
				overlay=frame.copy()
		if keyPress == 27:
			print('_____Data not accept..')
			print('*Select two points next press -a- to obtain Angle')
			#print('lista no append: '+str(lista))
			listaAux=[]
			listaAux1=[]
			#print('ListaAux Removed...'+ str(listaAux))
			frame=overlay.copy()
			cv2.imshow('First_Frame',frame)
		if keyPress == ord('a'):

			vrx=np.array([[listaSemFinal]],np.int32)
			pts=vrx.reshape((-1,1,2))
			cv2.polylines(frame,[pts],True,(0,255,255))
			cv2.line(frame,(listaAux[0]),(listaAux[1]),(255,0,0),2)
			##Angle:
			x1=listaAux[0][0]
			y1=listaAux[0][1]
			x2=listaAux[1][0]
			y2=listaAux[1][1]
			nume=-(y2-y1) # por la inversion que se maneja al emplear imagenes
			deno=x2-x1
			if deno ==0:
			    alpha = np.sign(nume)*90
			else:
			    alpha=math.atan(nume/deno)
			alpha=alpha*180/(math.pi)
			if (deno<0):
			    alpha=alpha+180
			#print('angule:.'+str(alpha))
			lista.append([int(alpha)])
			print('Press -q- to go a Full Resolution')
		if keyPress&0xFF==ord('q'):
			#print ('lista:  ---' +str(lista))
			break
	cv2.destroyAllWindows()
	#cap.release()
	#Capture of High Resolution
	try:
		cap=cv2.VideoCapture(directorioDeVideos+nameSourceVideo)
		#cap=cv2.VideoCapture('officialTrialVideos/sar.mp4')
		ret, frame1=cap.read()

	except:
		print('accediendo a imagen de placas...')
		try:
			frame1=cv2.imread(PathsAndNames.folderDeInstalacion+'/placa.jpg')
			frame=cv2.resize(frame1,(640,480))
			print('Se cargo imagen de Alta')
			imag=1
		except:
			print('No se encontro imagen...')
			imag=0
		if imag==0:
			try:
				cap=cv2.VideoCapture(1)
				cap.set(3,2560)
				cap.set(4,1920)
				ret, frame1=cap.read()
				frame=cv2.resize(frame1,(640,480))
				print('Se cargo video(1)')
			except:
				try:
					cap=cv2.VideoCapture(0)
					ret, frame1=cap.read()
					frame=cv2.resize(frame1,(640,480))
					print('Se cargo video(0)')
				except:
					frame=np.zeros((480,640),np.uint8)
					print('Se creo Mascara')
	cv2.namedWindow('FrameDeAltaResolucion')
	cv2.setMouseCallback('FrameDeAltaResolucion', get_BigRectangle)
	print('Select 2 points Press "q" to cut image')
	while(len(listaAux2)<3):
		cv2.imshow('FrameDeAltaResolucion',frame)
		keyPress = cv2.waitKey()
		if keyPress&0xFF==ord('q'):
			print('Full Resolution saved!!')
			break
	cv2.destroyAllWindows()
	print('select 2 points and Press "z" to zoom on traffic Light zone')
	frame=frame1[listCorteAltaRes[0][1]:listCorteAltaRes[1][1],listCorteAltaRes[0][0]:listCorteAltaRes[1][0]]
	imagenCortada=frame.copy()
	#print(frame1.shape)
	#print(frame.shape)
	#print(listCorteAltaRes)
	Const_X=frame.shape[1]/640
	Const_Y=frame.shape[0]/480
	frame=cv2.resize(frame,(640,480))
	listaAux2=[]
	cv2.namedWindow('FrameResolucion')
	cv2.setMouseCallback('FrameResolucion', get_Big_Sem_Polig)
	while(1):
		cv2.imshow('FrameResolucion',frame)
		keyPress = cv2.waitKey()
		if keyPress&0xFF==ord('z'):
			frame=frame[listaAux2[0][1]:listaAux2[1][1],listaAux2[0][0]:listaAux2[1][0]]
			Punto_X0=listaAux2[0][0]
			Punto_Y0=listaAux2[0][1]
			const_y_zoom=frame.shape[0]/240
			const_x_zoom=frame.shape[1]/320
			#print('Full Resolution TRAFFIC LIGHT saved!!')
			break
	cv2.destroyAllWindows()
	frame=cv2.resize(frame,(320,240))
	cv2.namedWindow('FrameToTrafficLight')
	cv2.setMouseCallback('FrameToTrafficLight', get_TrafficLight)
	print('select  traffic Light points and Press "q" to exit and save')
	while(1):
		cv2.imshow('FrameToTrafficLight',frame)
		keyPress = cv2.waitKey()
		if keyPress&0xFF==ord('q'):
			print('Full Resolution TRAFFIC LIGHT saved!!')

			break
	cv2.destroyAllWindows()
	while(1):
		vrx=np.array([[listaSemaAlta]],np.int32)
		pts=vrx.reshape((-1,1,2))
		cv2.polylines(imagenCortada,[pts],True,(0,255,255))
		for punto in lista[-1]:
			x = round(punto[0])
			y = round(punto[1])
			cv2.circle(imagenCortada, (x,y),2,(0,255,0),-1)

		cv2.imshow('imagen Final',cv2.resize(imagenCortada,(640,480)))
		keyPress = cv2.waitKey()
		if keyPress&0xFF==ord('q'):
			print('Done!!')
			break
	print('Standarizing data:')
	convertirAJSON(lista)

cv2.destroyAllWindows()

_Points(ev