import math
import numpy as np
from DisplayProxy import DisplayProxy
import time
from colorsys import hsv_to_rgb

CUBE_SIZE = 8
COLS = 20
ROWS = 12

def get_4x4_transform(trans_x, trans_y, trans_z):
    transform = [[1, 0, 0, trans_x],
                 [0, 1, 0, trans_y],
                 [0, 0, 1, trans_z],
                 [0, 0, 0, 1]]
    return np.matrix(transform)

xAngle = 0
yAngle = 0
zAngle = 0
def update_angle(i):
    global xAngle
    global yAngle
    global zAngle
    xAngle += 0.1
    yAngle += 0.1
    zAngle = 0

    rotX = np.matrix([
        [1, 0, 0, 0],
        [0, math.cos(xAngle), -math.sin(xAngle), 0],
        [0, math.sin(xAngle), math.cos(xAngle), 0],
        [0, 0, 0, 1]
      ])

    rotY = np.matrix([
        [math.cos(yAngle), 0, math.sin(yAngle), 0],
        [0, 1, 0, 0],
        [-math.sin(yAngle), 0, math.cos(yAngle), 0],
        [0, 0, 0, 1]
      ])

    rotZ = np.matrix([
        [math.cos(zAngle), -math.sin(zAngle), 0, 0],
        [math.sin(zAngle), math.cos(zAngle), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
      ])

    return rotX, rotY, rotZ

def inside_polygon(x, y, points):
    """
    Return True if a coordinate (x, y) is inside a polygon defined by
    a list of verticies [(x1, y1), (x2, x2), ... , (xN, yN)].

    Reference: http://www.ariel.com.au/a/python-point-int-poly.html
    """
    n = len(points)
    inside = False
    p1x, p1y = points[0]
    for i in range(1, n + 1):
        p2x, p2y = points[i % n]
        if y > min(p1y, p2y):
            if y <= max(p1y, p2y):
                if x <= max(p1x, p2x):
                    if p1y != p2y:
                        xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
                    if p1x == p2x or x <= xinters:
                        inside = not inside
        p1x, p1y = p2x, p2y
    return inside

cube_vertices = np.matrix([
        # 0 bottom left back
        [-CUBE_SIZE/2, -CUBE_SIZE/2, -CUBE_SIZE/2, 1],
        # 1 bottom right back
        [CUBE_SIZE/2, -CUBE_SIZE/2, -CUBE_SIZE/2, 1],
        # 2 top left back
        [-CUBE_SIZE/2, CUBE_SIZE/2, -CUBE_SIZE/2, 1],
        # 3 top right back
        [CUBE_SIZE/2, CUBE_SIZE/2, -CUBE_SIZE/2, 1],
        # 4 bottom left front
        [-CUBE_SIZE/2, -CUBE_SIZE/2, CUBE_SIZE/2, 1],
        # 5 bottom right front
        [CUBE_SIZE/2, -CUBE_SIZE/2, CUBE_SIZE/2, 1],
        # 6 top left front
        [-CUBE_SIZE/2, CUBE_SIZE/2, CUBE_SIZE/2, 1],
        # 7 top right front
        [CUBE_SIZE/2, CUBE_SIZE/2, CUBE_SIZE/2, 1],
    ])

sides = {'back': [0, 1, 3, 2], 'left': [0, 2, 6, 4], 'right': [1, 3, 7, 5], 'front': [4, 5, 7, 6],
    'bottom': [0, 1, 5, 4], 'top': [6, 7, 3, 2]}


side_colors = {
    'left': (244, 100, 100),
    'back': (75,75,75),
    'front': (144,144,120),
    'right': (0,0,125),
    'top': (21,144,2),
    'bottom': (12,211,144),
}

def distance(d1, d2):
    if len(d2) >= 3 and len(d1) >= 3:
        return math.sqrt((d1[0]-d2[0])**2+(d1[1]-d2[1])**2+(d1[2]-d2[2])**2)
    return math.sqrt((d1[0]-d2[0])**2+(d1[1]-d2[1])**2)


def get_color(side, i, coord, points):
    d = min([distance(points[s][:2], coord) for s in sides[side]]) / CUBE_SIZE
    #if side == 'left':
        #hue = ((math.sin(i/9) + 1)/2)
        #return [x*255 + 140 * (0.3 - d) for x in hsv_to_rgb(0.2, 1, 1)]
        #return [x*255 * d  for x in hsv_to_rgb(d-0.25, 1, 1)]
    #if side == 'right':
        #x = [p[0] for p in points]
        # = [p[1] for p in points]
        #centroid = (sum(x) / len(points), sum(y) / len(points))
        #centroid = (sum(x) / len(points), sum(y) / len(points))
        #return [x*255 * d  for x in hsv_to_rgb(d, 1, 1)]
    return [-50 + x - 300 * (0.2 - d) for x in side_colors[side]]


def background_plasma(i):
    w = COLS
    h = ROWS
    i /= 5
    for x in range (COLS):
        for y in range(ROWS):
            hue = math.sin(x/10 + math.sin(i/10)*10 + i/40) + 1
            hue += math.cos(y/10 + i/3) + 1
            hue /= 4.0
            hsv = hsv_to_rgb(hue, 1.0, 1.0)
            col = tuple([int(round(c * 255.0)) for c in hsv])
            display.set_pixel((x, y), [x * 0.02 for x in col])


points = []
i = 0
display = DisplayProxy()
while True:
    highest = [[0] * COLS for _ in range(ROWS)]
    display.set_fill((10,10,10))
    background_plasma(i)
    i+=1;
    rotX, rotY, rotZ = update_angle(i)
    trans = get_4x4_transform(9 + math.sin(i/10)*5, 6 + math.cos(i/10)*2, 10)
    coords = []
    points = []
    dist = [[0] * COLS for _ in range(ROWS)]
    for x in cube_vertices:
        x = x.dot(rotX)
        x = x.dot(rotY)
        x = trans.dot(x.T)
        (x, y, z, w) = x.T.tolist()[0]
        points.append((x, y, z, w))
        coords.append((int(x), int(y)))
    highest = [[0] * COLS for _ in range(ROWS)]
    for y in range(ROWS):
        for x in range(COLS):
            high_n = -1
            color = None
            for k, side in sides.items():
                stuff = [points[s][:2] for s in side]
                if inside_polygon(x, y, stuff):
                    for p in side:
                        if points[p][2] > high_n:
                            high_n = points[p][2]
                            color = k
            highest[y][x] = {'n': high_n, 'color': color}
            if color:
                display.set_pixel((x, y), get_color(color, i, (x,y, high_n), points))

    for side in sides:

        pass


    display.show()
    time.sleep(0.01)