#!/usr/bin/python3

## find short routes through 2D arrays
##
## Visegrady, Tamas  <tamas.visegrady@gmail.com>
## SPDX-License-Identifier: BSD-3-Clause


## pass X,Y in 2-column CSV, [0.0 .. 1.0]
##
## table in xy2path.tTABLE[] MUST have been generated by 2D/fractal generator
## it is indexed [0..2^N] in both dimensions

import fileinput, sys, math, csv, operator, itertools
import xy2path                     ## X+Y -> 1D fractal distance mapping; table
			## imports tTABLE[]

	## changes below threshold are written off as negligible
vEPS = 1E-9

vFLOATD = 6     ## float digits in sums


##--------------------------------------
## returns array of <x, y> pairs
## no normalization etc., beyond range checking
##
def table_read(fname, field=1):
	csvf = open(fname, newline='')
	rd   = list(csv.reader(csvf, delimiter=',', quotechar='\\'))
	res  = []

	for fi, fields in enumerate(rd):
		if len(fields) < 2:
			raise ValueError("missing XY columns (line {fi +1})")
		try:
			x, y = float(fields[0]), float(fields[1])
		except ValueError:
			raise ValueError("malformed input: "+
			                 f"[{fields[0]},{fields[1]}]")

		if not (0.0 <= x <= 1.0):
			raise ValueError(f"X out of range {x} (line {fi +1})")
		if not (0.0 <= y <= 1.0):
			raise ValueError(f"Y out of range {y} (line {fi +1})")

		res.append([x, y])

	return res


##--------------------------------------
def terminate(msg):
	sys.stderr.write(msg +'\n')
	sys.exit(-1)


##--------------------------------------
def usage():
	terminate("""
	Route 2D paths through space-filling functions
	Usage: [MAXN==...]  ... <name of input CSV containing X+Y coordinates>
	...usage blurb comes here...
	""")


##--------------------------------------
## works both for [x, y] (0.0 .. 1.0) and [x, y, x', y'] (2x2, incl. normalized)
##
def dumppath(p):
	print('\n'.join(f'{v[0]} {v[1]}' for v in p))


##--------------------------------------
## returns 2x floats normalized to [0 .. 2^N], plus curve-distance(x,y)
##
## TODO: check square table during reading
##
def xy2tablexy(x, y, table):
	"normalize [0.0 .. 1.0] coordinates to [0 .. 2^N] dimensions"

	xi, yi = x * len(table), y * len(table)

	return xi, yi, table[ round(xi) ][ round(yi) ]


##--------------------------------------
def xy2dist(xy1, xy2):
	return math.sqrt((xy1[0] - xy2[0]) **2 + (xy1[1] - xy2[1]) **2)


##--------------------------------------
## any array-of-tuples input with (x, y...) start is acceptable
## route is not closed: excludes loop-completing edge
##
def route2dists(xys):
	return (xy2dist(xys[i], xys[i+1])  for i in range(len(xys) -1))


##--------------------------------------
## any array-of-tuples input with (x, y...) start is acceptable
## route is not closed: excludes loop-completing edge
##
def route2total(xys):
	return sum(route2dists(xys))


##--------------------------------------
## tolerates, silently ignoring negative indexes
## repeated entries are counted only once
##
def idx2bitmask(arr):
	return sum((1 << v)  for v in set(arr)  if (v >= 0))


##----------------------------------------------------------------------------
## incremental swap costs, exchanging [i] and [j]:
##   --[i-1]-------[i]-------[i+1]--     --[i-1]       [i]       [i+1]---
##                                              \     /   \     /
##                                                \ /       \ /
##                                                 =         =
##                                                / \       / \
##                                              /     \   /     \
##   --[j-1]-------[j]-------[j+1]--     --[j-1]       [j]       [j+1]---
##                      d(X, Y) == distance(X .. Y)
##   swap [i] and [j]:
##      - removes  d(i-1, i) +d(i, i+1)
##      - removes  d(j-1, j) +d(j, j+1)
##      - adds     d(i-1, j) +d(j, i+1)
##      - adds     d(j-1, i) +d(i, j+1)


##--------------------------------------
## find swaps which may be performed simultaneously and reduce total
## route length the most: swap position of points [i] and [j]
##
## returns list of index pairs to swap
##         None  no improvement found during any of the swaps
##
## 'swaps' stores [index1, index2, bitmask(all affected indexes)]
## we can hypothetically swap all length-reducaing candidates which do not
## overlap (therefore the bitmasks)
##
def swap1(xys):
	swaps = []

	for i in range(1, len(xys)-3):
			## [i-1] -> [i] -> [i+1] -> [i+2]  swap, simplified:

		rem =  xy2dist(xys[ i-1 ], xys[ i   ])
		rem += xy2dist(xys[ i+1 ], xys[ i+2 ])
		add =  xy2dist(xys[ i-1 ], xys[ i+1 ])
		add += xy2dist(xys[ i   ], xys[ i+2 ])

		if (add -rem < 0):
			print(f'# {i}x({i}+1) +{add:.04f} -{rem:.04f} ' +
			      f'bal={add-rem:.06f}')
			sys.stdout.flush()
			swaps.append([i, i+1,
			              idx2bitmask([i-1, i,   i+1,
			                             i, i+1, i+2]),
			              add])

		for j in range(i+2, len(xys)-2):
			rem += xy2dist(xys[ i-1 ], xys[ i   ])
			rem =  xy2dist(xys[ i   ], xys[ i+1 ])
			rem += xy2dist(xys[ j-1 ], xys[ j   ])
			rem += xy2dist(xys[ j   ], xys[ j+1 ])

			add =  xy2dist(xys[ i-1 ], xys[ j   ])
			add += xy2dist(xys[ j   ], xys[ i+1 ])
			add += xy2dist(xys[ j-1 ], xys[ i   ])
			add += xy2dist(xys[ i   ], xys[ j+1 ])

			if (add -rem >= 0.0):
				continue

			print(f'# {i}x{j} +{add:.04f} -{rem:.04f} ' +
			      f'bal={add-rem:.06f}')
			sys.stdout.flush()

			swaps.append([i, j,
			              idx2bitmask([i-1, i, i+1,
			                           j-1, j, j+1]),
			              add -rem, ])

		## swaps[-1] is the current-best pair
		## check if any preceding swap pairs might be simultaneously
		## applied (their bitmasks do not overlap)
		##
		## need not be optimal here: leaving any un-swapped here will
		## still get considered in the next iteration.  therefore,
		## simply scanning backwards and collating all bitmasks
		## ('swapbm') without further filtering, is sufficient

	if swaps == []:
		return None

	swaps  = list(sorted(swaps, key=operator.itemgetter(3)))
	pairs, swapbm = [], 0

		## pick up pairs in decreasing-improvement order
		## ...if the new swap does not conflict with any
		## already accepted one
		##
	for x, y, bm, diff in reversed(swaps):
		if (swapbm & bm):
			continue
		swapbm |= bm
		pairs.append([x, y])
		print(f"## SWAP+ {x} {y}")

	return pairs


##--------------------------------------
## find reorders which improve total path lengths: moving [i] to
## to after [j] and before [j+1]:
##     [i-1] -> [i] -> [i+1] ... [j] ->        [j+1]   becomes
##     [i-1]        -> [i+1] ... [j] -> [i] -> [i+1]
##
## j >= i+2; j == i+1 has been covered by pairwise swaps
##
## returns list of index pairs to reorder, [i, j] tuples
##         None  no improvement found during any of the swaps
##
## 'reord' stores [index1, index2, bitmask(all affected indexes)]
## we can hypothetically swap all length-reducaing candidates which do not
## overlap (therefore the bitmasks)
##
def move1(xys):
	reord = []

	for i in range(1, len(xys)-3):
		for j in range(1, len(xys)-3):
			if (i == j) or (i+1 == j) or (i == j+1):
				continue

			rem =  xy2dist(xys[ i-1 ], xys[ i   ])
			rem += xy2dist(xys[ i   ], xys[ i+1 ])
			rem += xy2dist(xys[ j   ], xys[ j+1 ])
				##
			add =  xy2dist(xys[ i-1 ], xys[ i+1 ])
			add += xy2dist(xys[ j   ], xys[ i   ])
			add += xy2dist(xys[ i   ], xys[ j+1 ])

			if (add -rem) >= 0.0:
				continue

			print(f'# {i}->{j}+1 +{add:.04f} -{rem:.04f} ' +
			      f'bal={add-rem:.06f}')
			sys.stdout.flush()

			reord.append([i, j,
			             idx2bitmask([i-1, i, i+1, j, j+1]),
			             add -rem, ])
	if reord == []:
		return None

	reord  = list(sorted(reord, key=operator.itemgetter(3)))
	moves, movebm = [], 0

	for r in reord:
		i, j, bm = r[0], r[1], r[2]
		if (movebm & bm):
			continue
		movebm |= bm
		moves.append([i, j])
		print(f"## MOVE {i}->{j}+1")
	return moves


##--------------------------------------
## returns min(x), max(x), min(y), max(y)
##
def boundary_minmax(pt1, pt2, y=False):
	if y:
		return min(pt1[0], pt2[0]), max(pt1[0], pt2[0])

	return min(pt1[1], pt2[1]), max(pt1[1], pt2[1])


##--------------------------------------
## expect tests to mainly fail, with many points, so interleaved
## compute-check sequences are reasonable
##
def are_crossed_edges(s1, e1, s2, e2):

				## bounding boxes do not overlap -> not crossed

	minx1, maxx1 = boundary_minmax(s1, e1)
	minx2, maxx2 = boundary_minmax(s2, e2)

	if (maxx1 < minx2) or (minx1 > maxx2):
		return False

	miny1, maxy1 = boundary_minmax(s1, e1, y=True)
	miny2, maxy2 = boundary_minmax(s2, e2, y=True)

	if (maxy1 < miny2) or (miny1 > maxy2):
		return False

			## rebase to [i] as origin
			## check sign of [j,j+1] vs. [i,i+1] diagonal
			##
			## Fig.2
			##
			## TODO: proper elem-to-2D macros

			## cross product with j[1] and j+1[2]

	i2x, j1x, j2x = list((r - s1[0])  for r  in (e1[0], s2[0], e2[0]))
	i2y, j1y, j2y = list((r - s1[1])  for r  in (e1[1], s2[1], e2[1]))

	cp1 = i2x * j1y - i2y * j1x
	cp2 = i2x * j2y - i2y * j2x

	print(f"##1 {s1[0:2]} {e1[0:2]}")
	print(f"##2 {s2[0:2]} {e2[0:2]}")
	print(f"##x {cp1} {cp2}")

			## note: python lacks a sign() function
			## TODO: NaNs SHOULD be compared properly

	if (((cp1 >= 0) and (cp2 >= 0)) or
	    ((cp1 <= 0) and (cp2 <= 0))):
		return False                       ## segments do not intersect

	return True


##--------------------------------------
## swap	crossing edges to their non-crossing equivalents, if shorter
## (generally, crossed edges are longer)
##
## from:                          to:
##   --> 1      +- 3 <-- ...         --> 1 -----> 3 --> ...
##         \
##          -->-
##               \
##   <-- 4 -+      2 --> ...         <-- 4 <----- 2 <-- ...
##
## local permutations find uncrossing swaps, but only if they fit
## within the permutation window.  in the example above, if there are more
## than a few points betwen 2..3, they will not be permutation-detected
##
## since uncrossing reverses the 2..3 path order, it is more intrusive
## than simpler operations (although the paths are only reversed,
## not otherwise changed)
##
## TODO: numerical stability
##
def uncross1(xys):
	crosses = []

	for i in range(1, len(xys)-3):
		for j in range(i+2, len(xys)-3):
			if not are_crossed_edges(xys[i], xys[i+1],
			                         xys[j], xys[j+1]):
				continue

			print(f"## CROSS? {i}x{j}")

				## (1..2) -> (1..3)
				## (3..4) -> (2..4)

			rem =  xy2dist(xys[ i   ], xys[ i+1 ])
			rem += xy2dist(xys[ j   ], xys[ j+1 ])
			add =  xy2dist(xys[ i   ], xys[ j   ])
			add += xy2dist(xys[ i+1 ], xys[ j+1 ])

			if (add >= rem):
				continue
			print(f"## CROSS  {i}x{j}")

			crosses.append([i, j, add-rem])

	return None  if (crosses == []) else  crosses


##--------------------------------------
## returns number of improvements
##
def uncross1_process(xys, round, ucrs):
	imprd = 0

	if ucrs == None:
		return imprd

	route0 = route2total(xys)

	crs  = list(sorted(ucrs, key=operator.itemgetter(2)))

		## sections interfere
		## take only the best improvement in each round
		##
		## TODO: trivial to extend to non-overlapping ranges

	for p in crs:
		## ---> 1    +- 3 <-- ...   ==>   --> 1 -----> 3 --> ...
		## <-- 4 -+     2 --> ...   ==>   <-- 4 <----- 2 <-- ...
		##
			##       [i]  [i+1]       ...        [j  ]  [j+1]
			##       [i]  [j  ]  ...reversed...  [i+1]  [j+1]
			## ^^^^^^^^^                                ^^^^^^^^^
			## no change                                no change
		i, j = p[0], p[1]
		i, j = min(i, j), max(i, j)

		if j-i < 2:
			continue     ## should have been filtered @ enumeration

		xys[ i+1 : j+1 ] = list(xys[j : i : -1])

		curr = route2total(xys)
		if (curr > route0):
			print(f'## TOTAL.U??.{round}={ curr :.{vFLOATD}f}')
			raise ValueError("non-decreasing uncrossing")

		## TODO: generic 'is this a non-trivial improvement?' check
		## route0 = curr
		##
		## before that, MUST check that multiple uncrossings
		## do not interfere

		imprd += 1
		break

	report1(xys, round, 'U', 'uncross', cost0=route0)
	return imprd


##--------------------------------------
def permute_elems(elem_count, max=False):
	if max:
		return 7
	return 5


##--------------------------------------
## evaluate all permutations of N points for the shortest path through them
##
## returns [original start index, [...reordered indexes (n of them)...] ]
##         ...array of these tuples...
##         None  no improvement found during any of the swaps
##
def permute1(xys, n=4):
	perms = []

	for si in range(0, len(xys) -n -1):
		cost0, best = route2total(xys[ si+1 : si+n+1 ]), []
		cost = cost0

		for p in itertools.permutations(range(si+1, si+n+1)):
			pxy = list(xys[i] for i in p)
			c = xy2dist(xys[si], xys[ p[0] ])     + \
			    route2total(pxy)                  + \
			    xy2dist(xys[ p[-1] ], xys[ si+n+1 ])

			if (c >= cost -vEPS):
				continue
			cost, best = c, list(p)

		if best == []:
			continue

		perms.append([si, best, idx2bitmask(range(si, si+n+2)),
		              cost -cost0, ])

	if perms == []:
		return None

	perms  = list(sorted(perms, key=operator.itemgetter(3)))
	res, permsbm = [], 0

			## TODO: check this indexes are indeed permuted
			## (original order must not have been recognized as
			## decreasing, even without vEPS margin)

	for p in perms:
		i, plan, bm = p[0], p[1], p[2]
		if (bm & permsbm):
			continue

		permsbm |= bm
		res.append([i, plan])

		print(f"## PERM {i}->{ list(plan) } diff={ p[3] }")

	return res


##--------------------------------------
## returns number of improvements
##
def permute1_process(xys, round, perms):
	imprd = 0

	if perms == None:
		return imprd

	route0, n = route2total(xys), len(perms[0][1])
	best      = route0

	print(f"##PERMUTE.ROUND={round}")

	for i, j in perms:
		print(f"##PERM {i},(" +
		      f"{ ','.join(str(v) for v in j) }[{n}])")

		xys[ i+1:i+1+n ] = list(xys[v] for v in j)
		imprd += 1

		curr = route2total(xys)
		if (curr > route0):
			print(f'## TOTAL.P??.{round}={ curr :.{vFLOATD}f}')
			raise ValueError("non-decreasing " +
			                 "permutation")

		## TODO: generic 'is this an improvement?' check
		## route0 = curr

	report1(xys, round, 'P', 'permutation', cost0=route0)
	return imprd


##----------------------------------------------------------------------------
## non-None 'cost0' serves as upper bound, causing exception if over
##
def report1(xys, round, stageID, stage, cost0=None, logxy=False, brkline=False):
	if logxy:
		for p in xys:
			print(f'{p[2]},{p[3]}')

	cost = route2total(xys)

	if stageID != '':
		stageID = f'.{ stageID }'

	print(f'## TOTAL{stageID}.{round}={ cost :.6f}')
	if brkline:
		print()

	sys.stdout.flush()

	if (cost0 != None) and (cost > cost0):
		raise ValueError(f"non-decreasing {stage}")

	return cost


##----------------------------------------------------------------------------
if __name__ == '__main__':
	sys.argv.pop(0)
	if [] == sys.argv:
		usage()

	xys = table_read(sys.argv[0])
	xys = list([x, y, xy2tablexy(x, y, xy2path.tTABLE)[0],
	                  xy2tablexy(x, y, xy2path.tTABLE)[1],
	                  xy2tablexy(x, y, xy2path.tTABLE)[2],]
	           for x,y in xys)

	print("# INITIAL=")
	for p in xys:
		print(f'# {p[2]},{p[3]}')
	print()

			## initial plan: assign on curve
	xys = list(sorted(xys, key=operator.itemgetter(4)))

	round, dist0 = 0, route2total(xys)
	dist = dist0

	print(f"# SORTED.ROUND={round}")
	print(f"# TOTAL.0={ dist :.6f}")
	print(f"# DIST={ 100.0* dist / dist0 :.02f}%")
	##
	for p in xys:
		print(f'{p[2]},{p[3]}')
	print()

	while True:
		imprd, round = 0, round +1

		swaps = swap1(xys)
		if swaps != None:
			route0 = route2total(xys)
			best   = route0

			print(f"##SWAP1.ROUND={round}")
			for i, j in swaps:
				print(f"##SWAP {i},{j}")
				xys[i], xys[j] = xys[j], xys[i]
				imprd += 1

				curr = route2total(xys)
				if (curr > best):
					print(f'## TOTAL.S??.{round}={curr:.6f}')
					raise ValueError("non-decreasing swap")
				best = curr

			report1(xys, round, 'S', 'swap', route0)


			## unlike swaps, which preserve indexes, movement
			## needs to adapt as it shifts indexes: f.ex.,
			## moving (2 -> 5+1) removes element [2], so
			## a subsequent (3 -> 10+1) move would need to
			## remove [2] instead of [3] after the preceding
			## move
			##
			## 1) remove elements into temp array; replace
			##    them with placeholders.  mark if placeholders
			##    are touched again as they SHOULD NOT be
			##    (moves are non-overlapping)
			## 2) add elements in decreasing-index order
			## 3) discard placeholders

			## TODO: factor out move1_process()

		reord = move1(xys)
		if reord != None:
			route0 = route2total(xys)
			moved  = []    ## stores [target index, [...element...],
			               ##          orig.index]
			               ## orig. index is redundant, just for log

					## removal may proceed in arbitrary
					## order: entries only marked deleted
					## (set to None)
			for i, j in reord:
				if (j == i+1):
					raise ValueError("in-place reorder?")
				if (xys[i] == None):
					raise ValueError("overlapping moves? "+
					                 f"(element {i})")
				moved.append([j, xys[i], i])
				xys[i] = None                ## mark as removed

					## add back moved entries, decreasing
					## index order

			for m in sorted(moved, key=operator.itemgetter(0),
			                reverse=True):
				xys.insert(m[0]+1, m[1])
				imprd += 1

					## purge marked-as-deleted entries
			xys = list(x  for x in xys  if (x != None))

			curr = route2total(xys)
			if (curr > route0):
				print(f'## TOTAL.M??.{round}={ curr :.6f}')
				raise ValueError("non-decreasing move")
					##
					## TODO: cross-check cost decrease
					## after each movement?

			report1(xys, round, 'M', 'move', cost0=route0)

		perms = permute1(xys, n=permute_elems(len(xys)))
		if perms != None:
			imprd += permute1_process(xys, round, perms)

		uncrs = uncross1(xys)
		if uncrs != None:
			imprd += uncross1_process(xys, round, uncrs)

		report1(xys, round, '', '', logxy=True, brkline=True)

		if imprd == 0:
			break

		## TODO: uncross
		## crossing paths may not be detected by permutations
		## when they are far enough (counted along the 1D curve)


		## last try:
		## for 'sufficiently many' points still faster than N^2
		##
		## diags only: just look at stats (if uncommented)
		##
		## permutations may easily enumerated incrementally
		## please do not point this out to us
		##
	if False and (len(xys) > 2000):                   ## limit is arbitrary
		for n in range(permute_elems(len(xys))+1,
		               permute_elems(len(xys), max=True) +1):
			perms = permute1(xys, n)
			print(f"## n={n}, {perms}")
			sys.stdout.flush()

