import	math

# parameter for angles
pps = 25000
time_us_angle_zero = float(400) 				# it's just a random value, it must be tested (us = micro second)
time_us_per_point	= float(1000000 / pps)		# 40 us for 25kpps  (us = micro second)

# parmeter for brightness
max_lengh = 50									# in pixel, random value as well

import sys
def debug(*args, **kwargs):
    print(*args, file=sys.stderr, **kwargs)


def	different_position(p1, p2):
	return (int(p2[0]) != int(p1[0])) or (int(p2[1]) != int(p1[1]))

def	sub(p1, p2):
	return []

def	no_blanks_points(*points):
	for p in points:
		if p[2] == 0:
			return False
	return True

def	angle_exist(p1, p2, p3):
	return no_blanks_points(p1, p2, p3) and different_position(p1, p2) and different_position(p2, p3)

def	scalar_product(v1, v2):
	return v1[0] * v2[0] + v1[1] * v2[1]

def	length(v):
	return math.sqrt(scalar_product(v, v))

def	normalize(v):
	l = length(v)
	return [v[0] / l, v[1] / l]

def	sub(p1, p2):
	return [p1[0] - p2[0], p1[1] - p2[1]]

def	add(p1, p2):
	return [p1[0] + p2[0], p1[1] + p2[1]]

def	mult_by_scalar(p, scalar):
	return [p[0] * scalar, p[1] * scalar]

# 90 deg rotate to the right of v1 when y axes is directed to the floor
def	rot_90_right(v):
	return [-v[1], v[0]]

def	get_angle_degree(p1, p2, p3):
	v1 = sub(p2, p1)
	v2 = sub(p2, p3)
	d1x = normalize(v1)
	d1y = rot_90_right(d1x)
	x = scalar_product(d1x, v2)
	y = scalar_product(d1y, v2)
	angle = 180 - abs(math.degrees(math.atan2(y, x)))
	return angle
	
def	point_to_add(angle):
	return int(float((1 - (angle / 180.0)) * time_us_angle_zero) / time_us_per_point)

def	adjusts_angles(pl):
	if len(pl) < 3:
		return pl

	new_pl = []
	previous = pl[0]
	middle = pl[1]

	new_pl.append(pl[0])
	for ensuing in pl[2:]:
		if angle_exist(previous, middle, ensuing):
			angle = get_angle_degree(previous, middle, ensuing)
			nb_point_added = point_to_add(angle)
			for i in range(nb_point_added):
				new_pl.append(middle)
		new_pl.append(middle)
		previous = middle
		middle = ensuing
	new_pl.append(pl[-1])
	return new_pl

def	adjusts_brightness(pl):
	new_pl = []

	if len(pl) < 1:
		return pl
	new_pl.append(pl[0])
	prev = pl[0]
	for current in pl[1:]:
		new_pl.append(prev)
		if no_blanks_points(prev, current):
			diff = sub(current, prev)
			l = length(diff)
			if l > max_lengh:
				nb_add = math.ceil(l / max_lengh)
				print("\n\n===")
				print("from:", prev)
				print("to:", current)
				print("diff:", diff)
				for i in range(nb_add):
					factor = float(i) / float(nb_add)
					print("factor:", factor)
					new_point = add(prev, mult_by_scalar(diff, factor)) + [prev[2]]
					print(new_point)
					new_pl.append(new_point)
		#else:
		#	new_pl.append()
		prev = current
	new_pl.append(pl[-1])

	return new_pl

def	adjust_ending_point(pl):
	# si il y a trop de point noir, on en enleve
	# si il y a un que 1 point noir:
		# on en met un autre
	return pl

def	optimizer(pl):
	initial_length = len(pl)

	pl = adjusts_angles(pl)
	added_by_angle = len(pl) - initial_length

	pl = adjusts_brightness(pl)
	added_by_brightness = len(pl) - (added_by_angle + initial_length)

	#points_added = added_by_angle + added_by_brightness
	#print("\n\ninital_lenght:", initial_length, "\tadded by angles:", added_by_angle, "\tadded by brightness:", added_by_brightness )
	return pl

if __name__ == '__main__':
	# The 3 pl_somthing are used to test and debug the algorythm without
	# using all the programe archithecture. The goal is to be able to execute
	# the code snippet without errors being caught by LJ.
	pl_component = [(355, 262, 0), (355, 262, 16777215), (300, 130, 16777215), (432, 75, 16777215), (487, 207, 16777215), (355, 262, 16777215), (355, 262, 0), (0, 435, 0), (0, 480, 16777215), (0, 300, 16777215), (135, 300, 16777215), (135, 570, 16777215), (135, 570, 16777215), (135, 300, 16777215), (135, 300, 0), (135, 300, 16777215), (225, 480, 16777215), (225, 480, 0), (510, 450, 0), (510, 450, 16777215), (600, 420, 16777215), (600, 300, 16777215), (720, 300, 16777215), (720, 420, 16777215), (810, 450, 16777215), (810, 450, 0)]

	pl_brightness = [[50, 50, 0], [50, 50, 16777215], [50, 750, 16777215], [750, 750, 16777215], [750, 50, 16777215], [50, 50, 16777215], [50, 50, 0], [100, 100, 0], [100, 100, 16777215], [100, 700, 16777215], [700, 700, 16777215], [700, 100, 16777215], [100, 100, 16777215], [100, 100, 0], [150, 150, 0], [150, 150, 16777215], [150, 650, 16777215], [650, 650, 16777215], [650, 150, 16777215], [150, 150, 16777215], [150, 150, 0], [200, 200, 0], [200, 200, 16777215], [200, 600, 16777215], [600, 600, 16777215], [600, 200, 16777215], [200, 200, 16777215], [200, 200, 0], [250, 250, 0], [250, 250, 16777215], [250, 550, 16777215], [550, 550, 16777215], [550, 250, 16777215], [250, 250, 16777215], [250, 250, 0], [300, 300, 0], [300, 300, 16777215], [300, 500, 16777215], [500, 500, 16777215], [500, 300, 16777215], [300, 300, 16777215], [300, 300, 0], [350, 350, 0], [350, 350, 16777215], [350, 450, 16777215], [450, 450, 16777215], [450, 350, 16777215], [350, 350, 16777215], [350, 350, 0]]
	pl_angle = [[10.0, 83.0, 0], [10.0, 83.0, 16777215], [85, 83, 16777215], [160.0, 83.0, 16777215], [160.0, 83.0, 0], [160.0, 87.0, 0], [160.0, 87.0, 16777215], [85, 87, 16777215], [10.0, 87.0, 16777215], [10.0, 87.0, 0], [170.2853976431191, 76.46331929392564, 0], [170.2853976431191, 76.46331929392564, 16777215], [245, 83, 16777215], [319.7146023568809, 76.46331929392564, 16777215], [319.7146023568809, 76.46331929392564, 0], [319.7146023568809, 93.53668070607436, 0], [319.7146023568809, 93.53668070607436, 16777215], [245, 87, 16777215], [170.2853976431191, 93.53668070607436, 16777215], [170.2853976431191, 93.53668070607436, 0], [331.1394185240844, 69.97638667498022, 0], [331.1394185240844, 69.97638667498022, 16777215], [405, 83, 16777215], [478.8605814759156, 69.97638667498022, 16777215], [478.8605814759156, 69.97638667498022, 0], [478.8605814759156, 100.02361332501978, 0], [478.8605814759156, 100.02361332501978, 16777215], [405, 87, 16777215], [331.1394185240844, 100.02361332501978, 16777215], [331.1394185240844, 100.02361332501978, 0], [492.55556302831985, 63.58857161731095, 0], [492.55556302831985, 63.58857161731095, 16777215], [565, 83, 16777215], [637.4444369716801, 63.58857161731095, 16777215], [637.4444369716801, 63.58857161731095, 0], [637.4444369716801, 106.41142838268905, 0], [637.4444369716801, 106.41142838268905, 16777215], [565, 87, 16777215], [492.55556302831985, 106.41142838268905, 16777215], [492.55556302831985, 106.41142838268905, 0], [14.523053441056874, 217.34848925057486, 0], [14.523053441056874, 217.34848925057486, 16777215], [85, 243, 16777215], [155.47694655894313, 217.34848925057486, 16777215], [155.47694655894313, 217.34848925057486, 0], [155.47694655894313, 272.65151074942514, 0], [155.47694655894313, 272.65151074942514, 16777215], [85, 247, 16777215], [14.523053441056874, 272.65151074942514, 16777215], [14.523053441056874, 272.65151074942514, 0], [177.02691597225126, 211.30363036944755, 0], [177.02691597225126, 211.30363036944755, 16777215], [245, 243, 16777215], [312.97308402774877, 211.30363036944755, 16777215], [312.97308402774877, 211.30363036944755, 0], [312.97308402774877, 278.69636963055245, 0], [312.97308402774877, 278.69636963055245, 16777215], [245, 247, 16777215], [177.02691597225126, 278.69636963055245, 16777215], [177.02691597225126, 278.69636963055245, 0], [340.0480947161671, 205.5, 0], [340.0480947161671, 205.5, 16777215], [405, 243, 16777215], [469.9519052838329, 205.5, 16777215], [469.9519052838329, 205.5, 0], [469.9519052838329, 284.5, 0], [469.9519052838329, 284.5, 16777215], [405, 247, 16777215], [340.0480947161671, 284.5, 16777215], [340.0480947161671, 284.5, 0], [503.5635966783256, 199.98176727367155, 0], [503.5635966783256, 199.98176727367155, 16777215], [565, 243, 16777215], [626.4364033216743, 199.98176727367155, 16777215], [626.4364033216743, 199.98176727367155, 0], [626.4364033216743, 290.01823272632845, 0], [626.4364033216743, 290.01823272632845, 16777215], [565, 247, 16777215], [503.5635966783256, 290.01823272632845, 16777215], [503.5635966783256, 290.01823272632845, 0], [27.546666766076648, 354.79092927350956, 0], [27.546666766076648, 354.79092927350956, 16777215], [85, 403, 16777215], [142.45333323392336, 354.79092927350956, 16777215], [142.45333323392336, 354.79092927350956, 0], [142.45333323392336, 455.20907072649044, 0], [142.45333323392336, 455.20907072649044, 16777215], [85, 407, 16777215], [27.546666766076648, 455.20907072649044, 16777215], [27.546666766076648, 455.20907072649044, 0], [191.96699141100893, 349.96699141100896, 0], [191.96699141100893, 349.96699141100896, 16777215], [245, 403, 16777215], [298.03300858899104, 349.96699141100896, 16777215], [298.03300858899104, 349.96699141100896, 0], [298.03300858899104, 460.03300858899104, 0], [298.03300858899104, 460.03300858899104, 16777215], [245, 407, 16777215], [191.96699141100893, 460.03300858899104, 16777215], [191.96699141100893, 460.03300858899104, 0], [356.79092927350956, 345.54666676607667, 0], [356.79092927350956, 345.54666676607667, 16777215], [405, 403, 16777215], [453.20907072649044, 345.54666676607667, 16777215], [453.20907072649044, 345.54666676607667, 0], [453.20907072649044, 464.45333323392333, 0], [453.20907072649044, 464.45333323392333, 16777215], [405, 407, 16777215], [356.79092927350956, 464.45333323392333, 16777215], [356.79092927350956, 464.45333323392333, 0], [521.9817672736715, 341.5635966783256, 0], [521.9817672736715, 341.5635966783256, 16777215], [565, 403, 16777215], [608.0182327263285, 341.5635966783256, 16777215], [608.0182327263285, 341.5635966783256, 0], [608.0182327263285, 468.4364033216744, 0], [608.0182327263285, 468.4364033216744, 16777215], [565, 407, 16777215], [521.9817672736715, 468.4364033216744, 16777215], [521.9817672736715, 468.4364033216744, 0], [47.49999999999999, 498.0480947161671, 0], [47.49999999999999, 498.0480947161671, 16777215], [85, 563, 16777215], [122.5, 498.0480947161671, 16777215], [122.5, 498.0480947161671, 0], [122.5, 631.951905283833, 0], [122.5, 631.951905283833, 16777215], [85, 567, 16777215], [47.49999999999999, 631.951905283833, 16777215], [47.49999999999999, 631.951905283833, 0], [213.30363036944755, 495.02691597225123, 0], [213.30363036944755, 495.02691597225123, 16777215], [245, 563, 16777215], [276.69636963055245, 495.02691597225123, 16777215], [276.69636963055245, 495.02691597225123, 0], [276.69636963055245, 634.9730840277488, 0], [276.69636963055245, 634.9730840277488, 16777215], [245, 567, 16777215], [213.30363036944755, 634.9730840277488, 16777215], [213.30363036944755, 634.9730840277488, 0], [379.34848925057486, 492.52305344105685, 0], [379.34848925057486, 492.52305344105685, 16777215], [405, 563, 16777215], [430.65151074942514, 492.52305344105685, 16777215], [430.65151074942514, 492.52305344105685, 0], [430.65151074942514, 637.4769465589432, 0], [430.65151074942514, 637.4769465589432, 16777215], [405, 567, 16777215], [379.34848925057486, 637.4769465589432, 16777215], [379.34848925057486, 637.4769465589432, 0], [545.5885716173109, 490.55556302831985, 0], [545.5885716173109, 490.55556302831985, 16777215], [565, 563, 16777215], [584.4114283826891, 490.55556302831985, 16777215], [584.4114283826891, 490.55556302831985, 0], [584.4114283826891, 639.4444369716801, 0], [584.4114283826891, 639.4444369716801, 16777215], [565, 567, 16777215], [545.5885716173109, 639.4444369716801, 16777215], [545.5885716173109, 639.4444369716801, 0], [71.97638667498022, 649.1394185240844, 0], [71.97638667498022, 649.1394185240844, 16777215], [85, 723, 16777215], [98.02361332501978, 649.1394185240844, 16777215], [98.02361332501978, 649.1394185240844, 0], [98.02361332501978, 800.8605814759156, 0], [98.02361332501978, 800.8605814759156, 16777215], [85, 727, 16777215], [71.97638667498022, 800.8605814759156, 16777215], [71.97638667498022, 800.8605814759156, 0], [238.46331929392565, 648.285397643119, 0], [238.46331929392565, 648.285397643119, 16777215], [245, 723, 16777215], [251.53668070607435, 648.285397643119, 16777215], [251.53668070607435, 648.285397643119, 0], [251.53668070607435, 801.714602356881, 0], [251.53668070607435, 801.714602356881, 16777215], [245, 727, 16777215], [238.46331929392565, 801.714602356881, 16777215], [238.46331929392565, 801.714602356881, 0], [405.0, 648.0, 0], [405.0, 648.0, 16777215], [405, 723, 16777215], [405.0, 648.0, 16777215], [405.0, 648.0, 0], [405.0, 802.0, 0], [405.0, 802.0, 16777215], [405, 727, 16777215], [405.0, 802.0, 16777215], [405.0, 802.0, 0]] 

	optimizer(pl_brightness)


###########################################################################3333
# | Not used yet, it's for the eulerian graph optimization, probably comming soon =)
# v
#
#class	Node:
#	def	__init__(self, sid, color):
#		self.sid = sid
#		self.connected = []
#		self.used = False
#		self.color = color
#		self.is_free = None	# may be an other value to initialise
#		self.blank_connection = ""
#	
#	def add_nodes(self, neighbord):
#		not_the_same = neighbord != self.sid
#		not_allrady_inside = neighbord not in self.connected
#
#		if neighbord != self.sid and neighbord not in self.connected:
#			self.connected.append(neighbord)
#
#	# print the content of the objet to debug with print()
#	def __repr__(self):
#		is_free = "  \t###" if self.is_free else "  \t___"
#		return  is_free + str(self.connected) + "\n"
#
#class	Component:
#	def	__init__(self, all_nodes, comp_nodes):
#		self.all_nodes = all_nodes
#		self.id_nodes = comp_nodes
#		self.id_free_nodes = []
#		self.is_eulerian_circuit = True
#		# centre x,y
#		# bounding box
#		# other 
#		pass
#
#class	Graph:
#	nodes = {}	# dict of all nodes
#
#	def __init__(selt):
#		pass
#
#def	list_to_nodes(pl):
#	all_nodes = {}				# it will contain all nodes
#	sid_prev = None
#
#	for p in pl:
#		sid = str([int(p[0]), int(p[1])])
#		is_colored = p[2] != 0
#
#		if is_colored:
#			if sid not in all_nodes:
#				all_nodes[sid] = Node(sid, p[2])
#			if sid_prev != None:	
#				all_nodes[sid].add_nodes(sid_prev)
#				all_nodes[sid_prev].add_nodes(sid)
#		sid_prev = sid if is_colored else None
#	return all_nodes
#
## recursiv function witch get all connected node for one component and tag them as used
#def	get_one_comp(id_elem, nodes):
#	comp = []
#
#	comp.append(id_elem)
#	nodes[id_elem].used = True
#	for id_near in nodes[id_elem].connected:
#		if nodes[id_near].used == False:
#			comp += get_one_comp(id_near, nodes)
#	return comp
#
#def	get_comps(nodes):
#	comps = [] #all component
#	iter_nodes = iter(nodes)
#	nb_elem = len(nodes)
#
#	for id_nodes in iter_nodes:
#		if nodes[id_nodes].used == False:
#			comps.append(get_one_comp(id_nodes, nodes))
#	return comps
#
## if ther is a class for the component it would be a good idea to set en atribute about eulerian graph or non eulerian graph
#def	set_free_vertices(components, nodes):
#	for comp in components:
#		all_even_neighbord = True
#		for id_node in comp:
#			if len(nodes[id_node].connected) % 2 == 0: # test if even neighbord
#				nodes[id_node].is_free = False
#			else:
#				nodes[id_node].is_free = True
#				all_even_neighbord = False
#		if all_even_neighbord:
#			for id_node in comp:
#				nodes[id_node].is_free = True
#
#def	optimizer_old(pl):
#	all_nodes = {}				# it will contain all nodes
#	components = []	# list of connected node as a graph
#
#	# construct dict of connected all_nodes
#	all_nodes = list_to_nodes(pl)
#	components = get_comps(all_nodes)
#	set_free_vertices(components, all_nodes)
##@	print("\n\nall_nodes:\n", all_nodes)
##@	print("\n\nconnected_components:\n", components)
#	return pl
#