#!/usr/bin/python

import serial
from time import sleep
import thread
from subprocess import Popen

running=False
heartbeat=True
response=''
on=True; off=False

LH='lhip'; RH='rhip'
LK='lknee'; RK='rknee'
LA='lankle'; RA='rankle'
TM='timer'; CH='chopper'; SQ='squelch'; HM='home'; FLSH='flash'
commandnames=['hipx','hipy','knee']
ranges=[[0,255],[0,255],[0,255]]
poses=[['duck',[30,30],[80,100],[0,0]],['crane',[50,40],[5,5],[90,60]]]

aimos=serial.Serial('/dev/ttyACM0',57600)

aimos.setTimeout(1)
Popen(["stty","-inpck","-F","/dev/ttyACM0"]) #enable stubborn 8th bit after crashing

def test():
  global aimos,poses,heartbeat,response
  while not heartbeat:
    sleep(0.1)
  heartbeat=False
  send(LA,0,3,252)
  send(RA,0,3,252) 
  send(LA,10,3,253) 
  send(RA,10,3,253) 
  send(LA,20,3,254) 
  send(RA,20,3,254) 
  send(LA,30,3,255) 
  send(RA,30,3,255)
  response=''
  while response!='ok':
    response=''
    send(LA,100,3,255)
    sleep(1)
  print 'DONE!!!!'

def track(cx,cy,spd):                                       # test command to control a biped to track motion.
  global aimos,poses,heartbeat                              # used with another program that pipes into this terminal.
  while not heartbeat:                                      # this is how the AI is linked into the control susbystem.
    sleep(0.1)
  heartbeat=False
  n1,na1,nk1,nh1=poses[0]
  n2,na2,nk2,nh2=poses[1]
  ral=na1[0]-na2[0]; rar=na1[1]-na2[1]
  rkl=nk1[0]-nk2[0]; rkr=nk1[1]-nk2[1]
  rhl=nh1[0]-nh2[0]; rhr=nh1[1]-nh2[1]
  if ral>0: dal=min(na1[0],na2[0])+((abs(ral)/100.0)*cy)
  if ral<0: dal=max(na1[0],na2[0])-((abs(ral)/100.0)*cy)
  if rar>0: dar=min(na1[1],na2[1])+((abs(rar)/100.0)*cy)
  if rar<0: dar=max(na1[1],na2[1])-((abs(rar)/100.0)*cy)
  if rkl>0: dkl=min(nk1[0],nk2[0])+((abs(rkl)/100.0)*cy)
  if rkl<0: dkl=max(nk1[0],nk2[0])-((abs(rkl)/100.0)*cy)
  if rkr>0: dkr=min(nk1[1],nk2[1])+((abs(rkr)/100.0)*cy)
  if rkr<0: dkr=max(nk1[1],nk2[1])-((abs(rkr)/100.0)*cy)
  if rhl>0: dhl=min(nh1[0],nh2[0])+((abs(rhl)/100.0)*cy)
  if rhl<0: dhl=max(nh1[0],nh2[0])-((abs(rhl)/100.0)*cy)
  if rhr>0: dhr=min(nh1[1],nh2[1])+((abs(rhr)/100.0)*cy)
  if rhr<0: dhr=max(nh1[1],nh2[1])-((abs(rhr)/100.0)*cy)
  send(LA,dal,spd,255); send(RA,dar,spd,255)  
  send(LK,dkl,spd,255); send(RK,dkr,spd,255)  
  send(LH,dhl,spd,255); send(RH,dhr,spd,255)  
  
def send(cmdname,ps,spd,tck):                             # decodes a high level instruction into terminal command codes
  global aimos,joints,ranges,response
  if ps>=0 and ps<=100:
    if cmdname in commandnames:
      j=commandnames.index(cmdname)
      rmin,rmax=ranges[j]
      rg=float(rmax-rmin)
      if rg>0: p=min(rmin,rmax)+((abs(rg)/100.0)*ps)
      if rg<0: p=max(rmin,rmax)-((abs(rg)/100.0)*ps)
      msb=int(tck/256); lsb=tck-(msb*256)
      crc=65+j+msb+lsb+int(ps)+spd
      crcmsb=int(crc/256); crclsb=crc-(crcmsb*256)
      reponse=''; done=False; errctr=0
      aimos.write(chr(crcmsb)+chr(crclsb)+chr(65+j)+chr(int(ps))+chr(spd)+chr(msb)+chr(lsb))
#      while not done:
#        response=''
#        aimos.write(chr(crcmsb)+chr(crclsb)+chr(65+j)+chr(int(p))+chr(spd)+chr(msb)+chr(lsb))
#        while response=='': sleep(0.1)
#        if 'crc' in reponse: errctr+=1
#        if errctr>5: done=True
#        if 'ok' in response: done=True
      #print 'crc',crc,cmdname,int(ps),'(',int(p),')'

def savecfg():              # save config to flash
  global aimos
  send(FLSH,1,0,0)

def loadcfg():              # load config from flash
  global aimos
  send(FLSH,2,0,0)

def timers(onoff):          # control timers
  global aimos
  if onoff:
    send(TM,1,0,0)
  else:
    send(TM,2,0,0)

def envtimer(tl,b):         # set ticker length and period
  global aimos
  send(TM,6,0,tl)
  send(TM,7,0,b)
      
def report():               # get status report from sequencer
  global aimos
  send(TM,3,0,0)
      
def preset(tck):            # configure a new buffer setting
  global aimos
  send(TM,4,0,tck)
  
def reset():                # clear the buffer
  global aimos
  send(TM,5,0,0)

def chopper(m):             # set chopper delay
  global aimos
  send(CH,m,0,0)
  
def squelch(sq):            # squelch servos
  global aimos
  if sq!='none' and sq!='all': send(SQ,sq,0,0)
  if sq=='all': send(SQ,13,0,0)
  if sq=='none': send(SQ,14,0,0)

def home(s,v):              # set servo s home to angle v
  global aimos
  send(HM,s,1,v)

def minhome(s,v):           # set servo s min to angle v
  global aimos
  send(HM,s,2,v)

def maxhome(s,v):           # set servo s max to angle v
  global aimos
  send(HM,s,3,v)

def silent(onoff):          # control verbose
  global aimos
  if onoff: 
    send('silent',1,0,0)
  else:
    send('silent',0,0,0)
  
def quit():                 # safely shut down the system
  global running
  running=False

def portthread():                                         # reads serial port and puts sequencer responses into global variables
  global aimos,running,shell,heartbeat,pulse,response
  a=''
  while running==True:
    n=aimos.read()
    if n==chr(13): n=''
    if n:
      if n==chr(10):
        if a.strip():
          if not '#' in a:
            print a
            if 'ok' in a.strip(): response='ok'
            if a.strip()=='full': response='buffer full'
            if a.strip()=='err': response='error'
          else:
            heartbeat=True
            pulse=int(a.strip()[1:])
            #print pulse
            
        a=''
      else:
        a=a+n
    else: sleep(0.1)
  aimos.setDTR(False)       # resets the mcu remotely
  sleep(0.1)
  aimos.setDTR(True)
  n=' '
  while n:
    n=aimos.read()          # clears garbage from the boot
  aimos.close()
  
n=' '
while n:
  n=aimos.read()

aimos.setDTR(False)                         # reset mcu
sleep(0.1)
aimos.setDTR(True)

running=True
thread.start_new_thread(portthread,())      # run portthread in the background
sleep(3)

aimos.write(chr(255))                       # poke mcu
#sleep(2)

#track(0,50,255)

shell=False                                 # an interactive front end goes here, or runs from terminal. set shell true to drop to terminal
while running and not shell:                # and set running false to kill the port thread. use >> quit() for this.
  a=raw_input().strip()
  if a:
    if '>' in a:
      eval(a.split('>')[1].strip())
    else:
      if a=='shell': shell=True
      if a=='quit': quit()
      if 'report' in a: report()
#      if 'bend' in a: bend(a.split(' ')[1])
#      if 'twist' in a: twist(a.split(' ')[1])
#      if 'test' in a: test()
      if 'track' in a: track(int(a.split(' ')[1]),int(a.split(' ')[2]),255)
#      if 'shoulder' in a: shoulder(a.split(' ')[1])
      
if not shell:
  sleep(3)
else:
  print 'Interactive Python Shell'