import numpy as np
from PIL import Image, ImageDraw, ImageOps
import random
import operator

#etat initial test, sinon importer d'un fichier
map_size = [512, 512]

n_walls = 5
walls = []
walls.append(((0, 0), (0, 511)))
walls.append(((0, 511), (511, 511)))
walls.append(((511, 511), (511, 0)))
walls.append(((511, 0), (0, 0)))

walls.append(((0, 425), (140, 390)))
walls.append(((140, 390), (140, 125)))
walls.append(((140, 390), (315, 420)))
walls.append(((0, 320), (50, 280)))
walls.append(((140, 200), (360, 115)))
walls.append(((300, 220), (511, 140)))
walls.append(((185, 0), (260, 50)))

goal = (130, 440)
#robot = [(50, 50), (80, 80), (80, 45), (100, 70)] #test trajectoire
#i_robot = len(robot)-1 #indice de la position actuelle
robot = [(50, 50)] #trajectoire du robot
candidates = []
"""TODO: lecture de la mape, positions robot et but d'un fichier"""
"""TODO: sortie des methodes: nombre d'iterations, convergence"""

#---------------------------------------------------------------------------------------------------------
#fonctions de calcul

def perp(a):
	b = np.empty_like(a)
	b[0] = - a[1]
	b[1] = a[0]
	return b

#donne le point d'intersection de deux segments ou False si ils ne se coupent pas
def seg_intersect(c1, c2, d1, d2):
	a1 = np.array([float(c1[0]), float(c1[1])])
	a2 = np.array([float(c2[0]), float(c2[1])])
	b1 = np.array([float(d1[0]), float(d1[1])])
	b2 = np.array([float(d2[0]), float(d2[1])])
	da = a2 - a1
	db = b2 - b1
	dp = a1 - b1
	dap = perp(da)
	denom = np.dot(dap, db)
	if denom == 0:
		return False
	num = np.dot(dap, dp)
	p = (num / denom.astype(float)) * db + b1
	intersection = (int(round(p[0])), int(round(p[1])))
	if c1[0] < c2[0]:
		if intersection[0] < c1[0] or intersection[0] > c2[0]:
			return False
	elif c1[0] > c2[0]:
		if intersection[0] < c2[0] or intersection[0] > c1[0]:
			return False
	if d1[0] < d2[0]:
		if intersection[0] < d1[0] or intersection[0] > d2[0]:
			return False
	elif d1[0] > d2[0]:
		if intersection[0] < d2[0] or intersection[0] > d1[0]:
			return False
	if c1[1] < c2[1]:
		if intersection[1] < c1[1] or intersection[1] > c2[1]:
			return False
	elif c1[1] > c2[1]:
		if intersection[1] < c2[1] or intersection[1] > c1[1]:
			return False
	if d1[1] < d2[1]:
		if intersection[1] < d1[1] or intersection[1] > d2[1]:
			return False
	elif d1[1] > d2[1]:
		if intersection[1] < d2[1] or intersection[1] > d1[1]:
			return False
	return intersection

#calcule la distance euclidienne entre deux points
def dist_eucl(a, b):
	return np.linalg.norm(np.array(a) - np.array(b))

#verifie si le but est atteint
def goal_reached():
	i_robot = len(robot)-1
	if dist_eucl(robot[i_robot], goal) <= 10:
		print 'but atteint'
		return True
	goal_seg0 = ((goal[0] - 5, goal[1] - 5), (goal[0] + 5, goal[1] + 5))
	goal_seg1 = ((goal[0] - 5, goal[1] + 5), (goal[0] + 5, goal[1] - 5))
	if i_robot > 1:
		for i in range(i_robot):
			if (seg_intersect(robot[i], robot[i+1], goal_seg0[0], goal_seg0[1]) != False or seg_intersect(robot[i], robot[i+1], goal_seg1[0], goal_seg1[1]) != False):
				print 'but atteint'
				return True
	return False

#---------------------------------------------------------------------------------------------------------
#generation de candidats

#verifie qu'il n'y ait pas de mur entre le robot et une position candidate
def viable_candidate(p, i_robot):
	for w in walls[4:]:
		intersect_wall = seg_intersect(robot[i_robot], p, w[0], w[1])
		if intersect_wall != False:
			return False
	return True

#genere des positions candidates pour cette iteration, c-a-d des positions qui peuvent etre atteintes (a partir de la position actuelle du robot) en ligne droite sans passer par un mur
def generate_candidates(n_candidates):
	i_robot = len(robot)-1
	candidates = []
	candidates.append(robot[i_robot])
	for i in range(1, n_candidates):
		p_viable = False
		while p_viable == False:
			p = (random.randint(1, map_size[0]-1), random.randint(1, map_size[1]-1))
			p_viable = viable_candidate(p, i_robot) and p not in candidates
		candidates.append(p)
	return candidates
"""TODO: faire la generation de maniere plus intelligente qu'avec les random"""

#---------------------------------------------------------------------------------------------------------
#fitness

#calcule la fitness des positions candidats
def calculate_fitness(candidates):
	fitness = []
	for c in candidates:
		fitness.append(- dist_eucl(c, goal))
	return fitness

#methode fitness; nombre d'iterations limite car la methode risque de ne pas converger (c'est le cas adns l'exemple de l'article)
def fitness_search(n_candidates, max_iter):
	iter_count = 0
	while not goal_reached():
		candidates = generate_candidates(n_candidates)
		fitness = calculate_fitness(candidates)
		i_fit_max, fit_max = max(enumerate(fitness), key = operator.itemgetter(1))
		robot.append(candidates[i_fit_max])
		iter_count += 1
		if iter_count >= max_iter:
			print 'fitness search n\'a pas converge en ' + str(max_iter) + ' iterations'
			return iter_count
		print iter_count, fitness

#---------------------------------------------------------------------------------------------------------
#novelty

#calcule la nouveaute d'une position
def calculate_novelty(p, archive, population, k):
	n = len(archive) + len(population)
	if k > n:
		k = n
	distances = []
	for a in archive:
		distances.append(dist_eucl(p, a))
	for ind in population:
		distances.append(dist_eucl(p, ind))
	distances.sort()
	nov = 1.0 / k * sum([distances[i] for i in range(k)])
	return nov

#methode novelty
def novelty_search(n_candidates, k, min_nov, max_iter):
	archive = []
	iter_count = 0
	iter_no_add_count = 0
	while not goal_reached() and iter_count < max_iter:
		if iter_no_add_count > 2500:
			min_nov *= 0.95
		iter_count += 1
		iter_no_add_count += 1
		add_count = 0
		population = generate_candidates(n_candidates)
		novelties = []
		for i in range(n_candidates):
			novelties.append(calculate_novelty(population[i], archive, population, k))
			if novelties[i] >= min_nov:
				archive.append(population[i])
				add_count += 1
				iter_no_add_count = 0
		if add_count > 4:
			min_nov *= 1.05
		i_nov_max, nov_max = max(enumerate(novelties), key = operator.itemgetter(1))
		robot.append(population[i_nov_max])
		print iter_count
	return iter_count

#methode novelty sans archive
def novelty_search_no_archive(n_candidates, k, min_nov, max_iter):
	iter_count = 0
	iter_no_add_count = 0
	while not goal_reached() and iter_count < max_iter:
		if iter_no_add_count > 2500:
			min_nov *= 0.95
		iter_count += 1
		iter_no_add_count += 1
		add_count = 0
		population = generate_candidates(n_candidates)
		novelties = []
		for i in range(n_candidates):
			novelties.append(calculate_novelty(population[i], [], population, k))
			if novelties[i] >= min_nov:
				add_count += 1
				iter_no_add_count = 0
		if add_count > 4:
			min_nov *= 1.05
		i_nov_max, nov_max = max(enumerate(novelties), key = operator.itemgetter(1))
		robot.append(population[i_nov_max])
		print iter_count
	return iter_count

#methode novelty avec uniquement l'archive
def novelty_search_archive_only(n_candidates, k, min_nov, max_iter):
	archive = robot
	iter_count = 0
	iter_no_add_count = 0
	while not goal_reached() and iter_count < max_iter:
		if iter_no_add_count > 2500:
			min_nov *= 0.95
		iter_count += 1
		iter_no_add_count += 1
		add_count = 0
		population = generate_candidates(n_candidates)
		novelties = []
		for i in range(n_candidates):
			novelties.append(calculate_novelty(population[i], archive, [], k))
			if novelties[i] >= min_nov:
				archive.append(population[i])
				add_count += 1
				iter_no_add_count = 0
		if add_count > 4:
			min_nov *= 1.05
		i_nov_max, nov_max = max(enumerate(novelties), key = operator.itemgetter(1))
		robot.append(population[i_nov_max])
		print iter_count
	return iter_count

#---------------------------------------------------------------------------------------------------------
#MOEA

#construction du front de pareto en fonction des fitness et nouveaute de la population
def convert_to_pareto_front(pop_fit_nov):
	n = len(pop_fit_nov[0])
	i = 0
	while i < n-1:
		j = i + 1
		while j < n:
			#print i, j, n
			if pop_fit_nov[1][i] >= pop_fit_nov[1][j] and pop_fit_nov[2][i] >= pop_fit_nov[2][j]:
				del pop_fit_nov[0][j]
				del pop_fit_nov[1][j]
				del pop_fit_nov[2][j]
				n = len(pop_fit_nov[0])
			elif pop_fit_nov[1][i] <= pop_fit_nov[1][j] and pop_fit_nov[2][i] <= pop_fit_nov[2][j]:
				del pop_fit_nov[0][i]
				del pop_fit_nov[1][i]
				del pop_fit_nov[2][i]
				n = len(pop_fit_nov[0])
			j += 1
		i += 1
	return pop_fit_nov

#calcul de score sur les deux objectifs (fitness et nouveaute) en ponderant le rapport de chacun sur sa moyenne par un poids donne
def calculate_mo_score(pop_fit_nov, weight_obj0):
	weight_obj1 = 1 - weight_obj0
	n = len(pop_fit_nov[0])
	mo_scores = []
	mean_fit = 1.0 / n * sum(pop_fit_nov[1])
	mean_nov = 1.0 / n * sum(pop_fit_nov[2])
	for i in range(n):
		mo_scores.append(weight_obj0 * pop_fit_nov[1][i] / mean_fit + weight_obj1 * pop_fit_nov[2][i] / mean_nov)
	return mo_scores

#methode MOEA
def fit_nov_MOEA(n_candidates, k, min_nov, max_iter, weight_obj0):
	archive = []
	iter_count = 0
	iter_no_add_count = 0
	while not goal_reached() and iter_count < max_iter:
		if iter_no_add_count > 2500:
			min_nov *= 0.95
		iter_count += 1
		iter_no_add_count += 1
		add_count = 0
		pop_fit_nov = [[], [], []]
		pop_fit_nov[0] = generate_candidates(n_candidates)
		pop_fit_nov[1] = calculate_fitness(pop_fit_nov[0])
		pop_fit_nov[2] = []
		for i in range(n_candidates):
			pop_fit_nov[2].append(calculate_novelty(pop_fit_nov[0][i], archive, pop_fit_nov[0], k))
			if pop_fit_nov[2][i] >= min_nov:
				archive.append(pop_fit_nov[0][i])
				add_count += 1
				iter_no_add_count = 0
		if add_count > 4:
			min_nov *= 1.05
		#for i in range(len(pop_fit_nov[0])):
		#	print (pop_fit_nov[0][i], pop_fit_nov[1][i], pop_fit_nov[2][i])
		pop_fit_nov = convert_to_pareto_front(pop_fit_nov)
		mo_scores = calculate_mo_score(pop_fit_nov, weight_obj0)
		i_score_max, score_max = max(enumerate(mo_scores), key = operator.itemgetter(1))
		robot.append(pop_fit_nov[0][i_score_max])
		print iter_count
	return iter_count


#---------------------------------------------------------------------------------------------------------
#tests

random.seed()
n_candidates = 100
#iter_count, fitness = fitness_search(n_candidates, 50)
#iter_count = novelty_search(n_candidates, 5, 5, 50)
#iter_count = novelty_search_no_archive(n_candidates, 5, 5, 50)
#iter_count = novelty_search_archive_only(n_candidates, 5, 5, 50)
iter_count = fit_nov_MOEA(n_candidates, 5, 5, 500, 0.75)
#candidates = generate_candidates(n_candidates)
#print robot

#---------------------------------------------------------------------------------------------------------
#representation graphique

i_robot = len(robot)-1
im = Image.open('blank.png')
draw = ImageDraw.Draw(im)

for w in walls:
	draw.line(w, fill = 'black', width = 3)
draw.ellipse(((goal[0]-9, goal[1]-9), (goal[0]+9, goal[1]+9)), fill = 'black', outline = 'black') #but
for c in candidates:
	draw.ellipse(((c[0]-3, c[1]-3), (c[0]+3, c[1]+3)), fill = 'lightgray', outline = 'gray') #candidats
for i in range(len(robot)-1):
	draw.line((robot[i], robot[i+1]), fill = 'red') #trajectoire
draw.ellipse(((robot[0][0]-6, robot[0][1]-6), (robot[0][0]+6, robot[0][1]+6)), fill = 'white', outline = 'red') #position initiale du robot (debut de la trajectoire)
draw.ellipse(((robot[i_robot][0]-6, robot[i_robot][1]-6), (robot[i_robot][0]+6, robot[i_robot][1]+6)), fill = 'red', outline = 'black') #position actualle du robot (fin de la trajectoire)

del draw
im = ImageOps.flip(im)
im.save('test.png', 'PNG')
im.show()
#---------------------------------------------------------------------------------------------------------