#!/usr/bin/env python
#
# apaScan.py
#
# modified by Takuma Oami
# fablab Vestmanneyjar 13/5/14
#
# based on apa.py
#    APA serial interface
#
# Neil Gershenfeld
# CBA MIT 12/4/11
#
# (c) Massachusetts Institute of Technology 2010
# Permission granted for experimental and personal use;
# license for commercial sale available from MIT.
#


import serial, sys, time, wx

from wx import glcanvas

from OpenGL.GL import *
from OpenGL.GLUT import *

#
# define special chars
#
packet_start = '{'
packet_pointer = '^'
packet_divider = '|'
packet_end = '}'
packet_escape = '\\'
timeout_count = 1000
char_delay = 0.001
num_ports = 6 #number of ports each node has

#
# test for waiting chars
#
def test_waiting(ser):
   check_count = 0
   while 1:
      if (0 != ser.inWaiting()):
         return 1
      check_count += 1
      if (check_count == timeout_count):
         print "test_waiting: timeout"
         return 0
      time.sleep(char_delay)
#
# check for waiting chars and exit on timeout
#
def check_waiting(ser):
   check_count = 0
   while 1:
      if (0 != ser.inWaiting()):
         break
      check_count += 1
      if (check_count == timeout_count):
         print "check_waiting: timeout"
         sys.exit()
      time.sleep(char_delay)
   return
#
#send and get packet from APA network
#
def send_get_packet(ser, path, payload): 
    ser.flushInput()
    ser.flushOutput()
    #
    # send packet
    #
    packet = packet_start + packet_pointer + path + packet_divider + payload + packet_end
    print "send packet: "+packet
    for i in range(len(packet)):
        ser.write(packet[i])
        time.sleep(char_delay)
    #
    # get packet
    #
    packet = ""
    chr0 = ''
    count = 0
    while (chr0 != packet_start): # start
        if(0 == test_waiting(ser)):
            return -1
        chr0 = ser.read()
        count += 1
        if (count == timeout_count):
            print "apa.serial.test.py: timeout"
            sys.exit()
    packet += chr0
    chr0 = ''
    count = 0
    while (chr0 != packet_pointer): # pointer
        if(0 == test_waiting(ser)):
            return -1
        chr0 = ser.read()
        count += 1
        if (count == timeout_count):
            print "apa.serial.test.py: timeout"
            sys.exit()
        packet += chr0
    chr0 = ''
    count = 0
    while (chr0 != packet_divider): # divider
        if(0 == test_waiting(ser)):
            return -1
        chr0 = ser.read()
        count += 1
        if (count == timeout_count):
            print "apa.serial.test.py: timeout"
            sys.exit()
        packet += chr0
    chr0 = ''
    count = 0
    while (chr0 != packet_end): # end
        if(0 == test_waiting(ser)):
            return -1
        chr0 = ser.read()
        count += 1
        if (count == timeout_count):
            print "apa.serial.test.py: timeout"
            sys.exit()
        if (chr0 != packet_escape):
            packet += chr0
        else:
            if(0 == test_waiting(ser)):
                return -1
            chr0 = ser.read() # read escaped char
            packet += chr0
            if(0 == test_waiting(ser)):
                return -1
            chr0 = ser.read() # read next char
            packet += chr0
    print "receive packet: "+packet
    return packet
#
# Get positinon by path and origin position
#
def get_position(path, reverse_path, origin_type):
    #
    # faces directions for each types of positioning
    # there are three types of posiitoning
    # cube_map[type][x/y/z][back/forward]
    #
    cube_map= [[['1','3'], ['4','2'], ['0','5']],
               [['0','5'], ['1','3'], ['4','2']],
               [['4','2'], ['0','5'], ['1','3']]]
    position = [0,0,0]
    current_type = origin_type
    current_dir = [0,0]

    for i in range(len(path)):
        #
        # searching direction
        #
        for axis in range(3):
            for dir in range(2):
                if(cube_map[current_type][axis][dir] == path[i]):
                    current_dir[0] = axis
                    current_dir[1] = dir
        #
        # update curent position
        #
        position[current_dir[0]] = current_dir[1] * 2 - 1
        #
        # update current_type
        #
        for type in range(3):
            if(cube_map[type][current_dir[0]][current_dir[1]* -1 + 1] == reverse_path[len(reverse_path) - 2 - i]):
                current_type = type

    return position
        
#
# scan geometry of network
# return list named as node_list[] contains dictionary{id, position, path}
#
def scan_network(ser, node_list, path='', origin_type=0):
    current = {}
    #
    #check self
    #
    packet = send_get_packet(ser, path, 's')
    if(packet == -1):
        #
        #node doesn't exist
        #
        if(node_list == []):
            print "APA network isn't connected"
            sys.exit()
    else:
        #
        #node exists
        #
        root_port = packet[packet.find(packet_divider) + 1]
        reverse_path = packet[packet.find(packet_divider) + 1 : len(packet) - 1]
        current_position = get_position(path, reverse_path, origin_type)
        flg = 0
        for j in node_list:
           if(j['position'] == current_position):
              flg = 1
        if(flg != 1):
            current['id'] = len(node_list)
            current['path'] = path
            if(path != ''):
                current['position'] = current_position
            else:
                current['position'] = [0,0,0]
            node_list.append(current)
            for i in range(num_ports):
               if(i != int(root_port)):
                  scan_network(ser, node_list, path+str(i), origin_type)
#
# base class for openGL canvas
#
class MyCanvasBase(glcanvas.GLCanvas):

    def __init__(self, parent, list=None):

        glcanvas.GLCanvas.__init__(self, parent, -1)
        self.init = False
        self.context = glcanvas.GLContext(self)
        # initial mouse position
        self.lastx = self.x = 45
        self.lasty = self.y = 45
        self.size = None
        self.Bind(wx.EVT_ERASE_BACKGROUND, self.OnEraseBackground)
        self.Bind(wx.EVT_SIZE, self.OnSize)
        self.Bind(wx.EVT_PAINT, self.OnPaint)
        self.Bind(wx.EVT_LEFT_DOWN, self.OnMouseDown)
        self.Bind(wx.EVT_LEFT_UP, self.OnMouseUp)
        self.Bind(wx.EVT_MOTION, self.OnMouseMotion)
        self.node_list = list

    def OnEraseBackground(self, event):
        pass # Do nothing, to avoid flashing on MSW.

    """
    def OnSize(self, event):

        size = self.size = self.GetClientSize()
        if self.GetContext():
            self.SetCurrent()
            glViewport(0, 0, size.width, size.height)
        event.Skip()
    """
    def OnSize(self, event):
        wx.CallAfter(self.DoSetViewport)
        event.Skip()

    def DoSetViewport(self):
        size = self.size = self.GetClientSize()
        self.SetCurrent(self.context)
        glViewport(0, 0, size.width, size.height)

    def OnPaint(self, event):

        dc = wx.PaintDC(self)
        self.SetCurrent(self.context)
        if not self.init:
            self.InitGL()
            self.init = True
        self.OnDraw()


    def OnMouseDown(self, evt):

        self.CaptureMouse()
        self.x, self.y = self.lastx, self.lasty = evt.GetPosition()


    def OnMouseUp(self, evt):

        self.ReleaseMouse()


    def OnMouseMotion(self, evt):

        if evt.Dragging() and evt.LeftIsDown():
            self.lastx, self.lasty = self.x, self.y
            self.x, self.y = evt.GetPosition()
            self.Refresh(False)
#
# class for drawing cubes
#
class CubeCanvas(MyCanvasBase):

    def InitGL(self):

        # set viewing projection
        glMatrixMode(GL_PROJECTION)
        glFrustum(-0.5, 0.5, -0.5, 0.5, 1.0, 10.0)

        # position viewer
        glMatrixMode(GL_MODELVIEW)
        glTranslatef(0.0, 0.0, -5.0)

        # position object
        glRotatef(45, -1.0, 0.0, 0.0)
        glRotatef(45, 0.0, 0.0, 1.0)

        glEnable(GL_COLOR_MATERIAL)
        glEnable(GL_DEPTH_TEST)
        glEnable(GL_LIGHTING)
        glEnable(GL_LIGHT0)


    def OnDraw(self):

        # clear color and depth buffers
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

        # draw six faces of a cube
        glBegin(GL_QUADS)
        for i in self.node_list:
            pos_x = i['position'][1]
            pos_y = i['position'][0]
            pos_z = i['position'][2]

            glColor3f(0.0, 0.5, 0.5)
            if(i['position'] == [0,0,0]):glColor3f(0.5, 0.0, 0.0)
            #(0,0,1)
            #glColor3f(0.0, 0.0, 0.5)
            glNormal3f( 0.0, 0.0, 1.0)
            glVertex3f( 0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            glVertex3f(-0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            #(0,0,-1)
            #glColor3f(0.0, 0.5, 0.5)
            glNormal3f( 0.0, 0.0,-1.0)
            glVertex3f(-0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
            glVertex3f( 0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            #(0,1,0)
            #glColor3f(0.0, 0.5, 0.0)
            glNormal3f( 0.0, 1.0, 0.0)
            glVertex3f( 0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            glVertex3f( 0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            #(0,-1,0)
            #glColor3f(0.5, 0.5, 0.0)
            glNormal3f( 0.0,-1.0, 0.0)
            glVertex3f(-0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            glVertex3f(-0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            #(1,0,0)
            #glColor3f(0.5, 0.0, 0.0)
            glNormal3f( 1.0, 0.0, 0.0)
            glVertex3f( 0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            glVertex3f( 0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            glVertex3f( 0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
            #(-1,0,0)
            #glColor3f(0.5, 0.0, 0.5)
            glNormal3f(-1.0, 0.0, 0.0)
            glVertex3f(-0.5+pos_x,-0.5+pos_y,-0.5+pos_z)
            glVertex3f(-0.5+pos_x,-0.5+pos_y, 0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y, 0.5+pos_z)
            glVertex3f(-0.5+pos_x, 0.5+pos_y,-0.5+pos_z)
        glEnd()

        if self.size is None:
            self.size = self.GetClientSize()
        w, h = self.size
        w = max(w, 1.0)
        h = max(h, 1.0)
        xScale = 180.0 / w
        yScale = 180.0 / h
        glRotatef((self.y - self.lasty) * yScale, 1.0, 0.0, 0.0);
        glRotatef((self.x - self.lastx) * xScale, 0.0, 1.0, 0.0);

        self.SwapBuffers()
#
#MAIN
#
if __name__ == '__main__':
    #
    # get input
    #
    if (len(sys.argv) != 3):
        print "command line: python apa.py serial_port port_speed"
        sys.exit()
    serial_port = sys.argv[1]
    port_speed = int(sys.argv[2])
    nodes=[]
    #
    # open port
    #
    ser = serial.Serial(port=serial_port,baudrate=port_speed)

    scan_network(ser, nodes)
    
    for i in nodes:
        print i
    
    app = wx.App(0)

    frame = wx.Frame(None, -1, size=(800,800))
    canvas = CubeCanvas(frame,list=nodes)
    frame.Show(True)
    
    app.MainLoop()