Xuanwu path finder

from laby import *
from math import atan2, sqrt, pi
import numpy as np

def try_advance(pos, dist):
  state = list(pos)
  dx, dy = cos(state[2]), sin(state[2])
  while(dist > 0):
    x, y = state[0] + dx/4, state[1] + dy/4
    ix, iy = int(x) - (x < 0), int(y) - (y < 0)
    drx, dry = ix - int(state[0]), iy - int(state[1])
    vw = lambda y, x: wl[y] & 2**x
    hw = lambda y, x: wl[y + laby_h] & 2**x
    wx = laby_w - 2 - min(ix, ix - drx)
    tx = drx and (ix < 0 or ix >= laby_w or vw(iy - dry, laby_w - 2 - min(ix, ix - drx)))
    ty = dry and (iy < 0 or iy >= laby_h or hw(min(iy, iy - dry), laby_w - 1 - (ix - drx)))
    t = dx <= 0 or int(x) < laby_w - 1 or int(y) < laby_h - 1
    if t and tx or ty or (drx and dry and (t and tx or ty)) or (drx and dry and (t and vw(iy, laby_w - 2 - min(ix, ix - drx)) or hw(min(iy, iy - dry), laby_w - 1 - ix))):
      return False
    dist -= .25
    state[0:1] = (x, y)
  return True

def get_positions(pos_list, target_pos):
    result = []
    count = 0
    for pos in pos_list:
        count += 1
        if count % 1000 == 0: print(count)
        for da10 in range(-99, 100):
            da = da10 / 10.
            new_angle = pos[2] + da
            rot_pos = (pos[0], pos[1], new_angle)
            for dist10 in range(-3, 3):
                dx = target_pos[0] - pos[0]
                dy = target_pos[1] - pos[1]
                dist = sqrt(dx**2 + dy**2)
                dist += dist10 / 2.
                dist = round(dist * 2) / 2
                new_pos = (pos[0] + dist * cos(new_angle), pos[1] + dist * sin(new_angle), new_angle, da, dist, pos[5] + [pos[0:5]])
                if abs(new_pos[0] - target_pos[0]) < .75 and abs(new_pos[1] - target_pos[1]) < .75:
                    if try_advance(rot_pos, dist):
                        result.append(new_pos)
                    else:
                        break
    return result

manual_positions = [
    (2.5, 1.5),
    (4, 7.5),
    (8.75, 8.75),
    (9.5, 5),
    (14.75, 3.6),
    (16.2, 8.5),
    (17.2, 9),
    (16, 10.9),
    (40, 11.9)
]

colors = [
    (0, 0x64, 0),
    (0xbc, 0x8f, 0x8f),
    (0xff, 0, 0),
    (0xff, 0xd7, 0),
    (0, 0xff, 0),
    (0, 0xff, 0xff),
    (0xa0, 0x20, 0xf0),
    (0x1e, 0x90, 0xff),
    (0xff, 0x14, 0x93)
]

def pos_list(pos):
    return pos[5][1:] + [pos[0:5]]

import cv2, numpy as np
screen_w, screen_h = 640, 480
w_scale, h_scale = 640 / 20, 480 / 12
img = np.full((screen_h, screen_w, 3), 255, np.uint8)

positions = [(0., 0.5, 0., 0., 0., [])]
for index, new_pos in enumerate(manual_positions):
    print(index, ':', len(positions))
    positions = get_positions(positions, new_pos)
    for pos in positions:
        for p in pos_list(pos):
            try:
                img[int(p[1] * h_scale)][int(p[0] * w_scale)] = colors[index]
            except: pass

print(len(manual_positions), ':', len(positions))

def sign_score(pos, signs):
    total = 0
    for i, x in enumerate(pos_list(pos)):
        total += x[4]
        total += x[3] * signs[i]
    return 72 + sin(total), total

def score(pos):
    min_score = 73
    for i in np.ndindex((2, 2, 2, 2, 2, 2, 2, 2, 2)):
        signs = np.subtract(np.multiply(i, 2.0), 1.0)
        s, total = sign_score(pos, signs)
        if s < min_score:
            min_score = s
            min_signs = signs
            min_total = total
    return min_score, min_signs, min_total % (pi*2) - 3*pi/2

def get_orders(pos, s):
    score, signs, total = s
    return ([(x[3], x[4], sign) for x, sign in zip(pos_list(pos), signs)], score, total)

with open('out.txt', 'w') as f:
    orders = []
    for i, pos in enumerate(positions):
        if i % 1000 == 0: print(i)
        orders.append(get_orders(pos, score(pos)))
    print('sorting...')
    orders.sort(key= lambda x: x[1])
    f.write(str(orders))

cv2.imshow('Xuanwu', img)
cv2.waitKey(0)