#This is the code for the "Intel Edison LightPlotter" on instructables by Jason "Ossum" Suter, feel free to do whatever you please with it

import time
import pyupm_grovemd as upmGrovemd
import cmd
from math import sqrt
from xml.dom import minidom
import urllib2
from threading import Event, Thread
import re

class lightPen:
  
  def __init__(self,ipAddress):
    self.ipAddress = ipAddress
    self.red = 0
    self.green = 0
    self.blue = 0
  
  def setColour(self):
    #this version of the function sets the LED via a threaded function, meaning that there doesnt need to be a pause between steps (however, the light could come on marginally after it starts moving)
    print "setting colour",self.red,self.green,self.blue
    urlString = 'http://'+self.ipAddress+'/?RED='+str(self.red)+'&GREEN='+str(self.green)+'&BLUE='+str(self.blue)
    urllib2.urlopen(urlString)  
    
  def setColourRGBTuple(self,rgb):
    #rgb values are 0-255
    try:
      self.red = max(min(int(rgb[0]),255),0)
      self.green = max(min(int(rgb[1]),255),0)
      self.blue = max(min(int(rgb[2]),255),0)
      Thread(target=self.setColour,args=()).start()
      
    except:
      print "setColourRGBTuple arror"
    
  def setColourHexString(self,rgb):
    #string of form '#rrggbb'
    try:
      self.red = max(min(int(rgb[1:3],16),255),0)
      self.green = max(min(int(rgb[3:5],16),255),0)
      self.blue = max(min(int(rgb[5:7],16),255),0)
      Thread(target=self.setColour,args=()).start()
    except:
      print "setColourHexString error"
    

    
class svgHandler:
  
  def __init__(self):
    self.filename = ""
    self.lines = [] #list of segments of the form [x0 y0 x1 y1 RGB] where x and y are 0-1 floats
  
  def importFile(self,filename):
    doc = minidom.parse(filename)  # parseString also exists
    paths = doc.getElementsByTagName('path')
    pathsandcolours = [] 
    self.lines = []
    
    """
    Go through the paths and store their coordinate strings and colours in a list of tuples
    """
    for p in paths:
      styleValues =  p.attributes['style'].value.split(";")
      for val in styleValues:
        if val.split(":")[0] == 'stroke':
          rgb = val.split(":")[1]     
      pathsandcolours.append((p.getAttribute('d'),rgb))

    """
    For each path and its corresponding colour, convert to a set of line segments of the form [x0 y0 x1 y1 RGB]
    """
    xMin = float("inf")
    xMax = -float("inf")
    yMin = float("inf")
    yMax = -float("inf")
    unScaledLines = []
    
    for path,rgb in pathsandcolours:
      pathCoords = re.split(r'([CcLlMmZz])',path)
      startX = 0
      startY = 0
      lastX = 0
      lastY = 0
      newLine = True
      closeLine = False
      #we are assuming absolute coordinates in the SVG
      
      print pathCoords
      for coord in pathCoords:
        coord = coord.strip()
        if len(coord) == 0:
          pass
        elif coord == 'M': #'M' is move cursor to absolute position
          newLine = True
        elif coord == 'L': #'L' is draw line to absolute position
          newLine = False
        elif coord == 'Z' or coord.strip() == 'z': #close line to initial point
          closeLine = True
        else:
          try:
            
            
            x = float(re.split(r'[\s,]',coord)[0])
            y = float(re.split(r'[\s,]',coord)[1])
            xMin = min(xMin,x)
            yMin = min(yMin,y)
            xMax = max(xMax,x)
            yMax = max(yMax,y)

            if closeLine:
              lastX = x
              lastY = y
              startX = x
              startY = y
              unScaledLines.append([lastX,lastY,startX,startY,rgb])
            elif newLine:
              lastX = x
              lastY = y
              startX = x
              startY = y
            else:
              unScaledLines.append([lastX,lastY,x,y,rgb])
              lastX = x
              lastY = y
          except:
            print "unhandled command: ",coord
        
      #print "xMin,yMin",xMin,yMin
      #print "xMax,yMax",xMax,yMax        
    
    xTotal = xMax-xMin
    yTotal = yMax-yMin
    
    
    for x0,y0,x1,y1,rgb in unScaledLines:
      X0normalised = (x0-xMin)/xTotal
      X1normalised = (x1-xMin)/xTotal
      Y0normalised = (y0-yMin)/yTotal
      Y1normalised = (y1-yMin)/yTotal
      self.lines.append([X0normalised,Y0normalised,X1normalised,Y1normalised,rgb])
        
  def plotPath(self):
    import matplotlib.pyplot as plt
    plt.gca().invert_yaxis()
    for x0,y0,x1,y1,rgb in self.lines:
      print int(rgb[1:3],16),int(rgb[3:5],16),int(rgb[5:7],16)
      plt.plot([x0,x1],[y0,y1],color=rgb)
    plt.savefig(self.filename.split('.')[0]+'.png')
    
    
    
    
    

class lightPlotter:

  def __init__(self,motor1, motor2, horizontalSteps=1700,stepTime=0.01):
    self.motor1 = motor1
    self.motor2 = motor2
    self.xRes = horizontalSteps
    self.stepTime = stepTime
    self.stepTimeMax = 1.0
    self.usableArea = 0.85 #a percentage value that indicates the usable area of the plotter. Image will be scaled to fit in here
    self.yOffset = 0
    
  def setOrigin(self):
    self.motor1.stepCount = 0
    self.motor2.stepCount = self.xRes

  def goTo(self,x,y):
    #x and y are floats between 0 and 1
    #we scale and shift them to fit in the usable area
    x = self.usableArea*x + (1.0-self.usableArea)/2.0
    y = self.usableArea*y + (1.0-self.usableArea)/2.0 + self.yOffset
    
    #s1 and s2 the string lengths required
    s1 = int(sqrt(x**2 + y**2)*self.xRes)
    s2 = int(sqrt((1-x)**2 + y**2)*self.xRes)

    #these are the number of steps required to get there
    m1steps = s1 - self.motor1.stepCount
    m2steps = s2 - self.motor2.stepCount
    
    #print "go to s1:",s1,"from:",self.motor1.stepCount," delta steps:",m1steps
    #print "go to s2:",s2,"from:",self.motor2.stepCount," delta steps:",m2steps

    #we scale the speed. one motor always runs at "max speed" as set in lightPlotter class
    #the other motor is slowed so that is reaches the end at the same time.
    #this is still going to result in curvy lines, but at least not jagged ones.
    #we can resample long lines as short ones to counter this effect easily
    longPath = max(abs(m1steps),abs(m2steps))
    
    if m1steps != 0:
      m1interval = min(abs(self.stepTime*(longPath/float(m1steps))),self.stepTimeMax)
    else:
      m1interval = self.stepTime
    
    if m2steps != 0:
      m2interval = min(abs(self.stepTime*(longPath/float(m2steps))),self.stepTimeMax)
    else:
      m2interval = self.stepTime
      
    #this isn't working right now, need to keep it within the steppers "good range" too
    #I think this basic scaling was resulting in some overly long steps.
    #self.runSteppers(m1steps,m2steps,m1interval,m2interval)
    print "interval 1, interval 2:",m1interval,m2interval
    self.runSteppers(m1steps,m2steps,m1interval,m2interval)

 
  def runSteppers(self,m1steps,m2steps,m1interval,m2interval):
    """
    this is a threaded function that will run both motors, at different steps and intervals, at the same time
    """
    m1 = self.motor1
    m2 = self.motor2
    m1dir = True
    m2dir = True

    if m1steps < 0:
      m1dir = False
      m1steps = abs(m1steps)

    if m2steps < 0:
      m2dir = False
      m2steps = abs(m2steps)
      
    stopped = Event()
    
    
    def loop(motor,steps,interval,dir):
      while not stopped.wait(interval) and steps > 0: # the first call is in `interval` secs
        steps -= 1
        motor.step(dir)
    
    m1Thread = Thread(target=loop,args=(m1,m1steps,m1interval,m1dir))   
    m2Thread = Thread(target=loop,args=(m2,m2steps,m2interval,m2dir))  
    
    m1Thread.start()
    m2Thread.start()
      
    m1Thread.join()
    m2Thread.join()
    
    return stopped.set
      
  def stepMotors(self,m1steps,m2steps):
    #m1 and m2 number of steps per motor, positive or negative
    dir1 = True
    dir2 = True

    if m1steps < 0:
      dir1 = False
      m1steps = abs(m1steps)

    if m2steps < 0:
      dir2 = False
      m2steps = abs(m2steps)

    while (m1steps > 0) or (m2steps > 0):
      if m1steps > 0:
        self.motor1.step(dir1)
        m1steps -= 1
      if m2steps > 0:
        self.motor2.step(dir2)
        m2steps -= 1
      time.sleep(self.stepTime)


class stepper:
  steps = [[2,2],[2,0],[2,1],[0,1],[1,1],[1,0],[1,2],[0,2]]
  nextStep = 0
  direction = 1 #1 or -1 #can be inverted to invert motor direction
  stepCount = 0

  def __init__(self, I2C_ADDR, I2C_BUS,direction):
    self.motorDriver = upmGrovemd.GroveMD(I2C_BUS, I2C_ADDR)
    self.motorDriver.setMotorSpeeds(255,255)
    time.sleep(0.05)
    self.direction=direction

  def step(self,lengthen):
    """
    Take one step in either direction.
    Takes a boolean argument to indicate lengthen or shortening string
    """
    if (lengthen):
      self.stepCount += 1
      self.nextStep += self.direction
    else:
      self.stepCount -= 1
      self.nextStep -= self.direction
    
    if self.nextStep > 7:
      self.nextStep = 0
    if self.nextStep < 0:
      self.nextStep = 7
    self.motorDriver.setMotorDirections(self.steps[self.nextStep][0],self.steps[self.nextStep][1])


  def disable(self):
    self.motorDriver.setMotorSpeeds(0,0)

  def enable(self):
    self.motorDriver.setMotorSpeeds(255,255)

class cmdInterface(cmd.Cmd):
  intro = "Lightplotter Command Interface"
  m1 = stepper(0x0a,upmGrovemd.GROVEMD_I2C_BUS,-1)
  m2 = stepper(0x0f,upmGrovemd.GROVEMD_I2C_BUS,-1)
  plotter = lightPlotter(m1, m2)
  pen = lightPen('192.168.42.2')
  svgH = svgHandler()
  
  def do_plotsvg(self,args):
    try:
      fname = args.split()[0]
      self.svgH.importFile(fname)
    except:
      print "import error"

    lastX = 0
    lastY = 0
    lastRGB = '#000000' 
    for x0,y0,x1,y1,rgb in self.svgH.lines:
      if x0 != lastX or y0 != lastY:
        #go to beginning of line if we aren't there already
        self.pen.setColourHexString('#000000')
        self.plotter.goTo(x0,y0)
        lastRGB = '#000000'
      
      if rgb != lastRGB:
        #if the new line is not the same colour as the last one
        self.pen.setColourHexString(rgb)
        lastRGB = rgb
      
      #now draw the new line
      self.plotter.goTo(x1,y1)
      lastX = x1
      lastY = y1
    
    #we are finished drawing the SVG. Turn off LED
    self.pen.setColourHexString('#000000')
    

      
  def do_movesteps(self, args):
    """
    moveX [motor] [steps]
    motor: 1 or 2
    steps: + or - integer
    """
    argsOk = True
    try:
      m1steps = int(args.split()[0])
      m2steps = int(args.split()[1])
    except:
      print "invalid arguments"

    self.plotter.stepMotors(m1steps,m2steps)

  def do_invertMotor(self,args):
    try:
      motor = int(args.split()[0])
    except:
      print "invalid arguments"

    if (motor == 1):
      self.m1.direction = -self.m1.direction
    elif (motor == 2):
      self.m2.direction = -self.m2.direction
    else:
      print "invalid motor number"

  def do_disableMotor(self, args):
    """
    disable [motor]
    motor: 1 or 2 or 3 (both)
    """
    try:
      motor = int(args.split()[0])
    except:
      print "invalid arguments"

    if (motor == 1):
      self.m1.disable()
    elif (motor == 2):
      self.m2.disable()
    elif (motor == 3):
      self.m1.disable()
      self.m2.disable()

  def do_enableMotor(self, args):
    """
    disable [motor]
    motor: 1 or 2 or 3 (both)
    """
    try:
      motor = int(args.split()[0])
    except:
      print "invalid arguments"

    if (motor == 1):
      self.m1.enable()
    elif (motor == 2):
      self.m2.enable()
    elif (motor == 3):
      self.m1.enable()
      self.m2.enable()

  def do_setrgb(self,args):
    try:
      r = max(min(int(args.split()[2]),255),0)
      g = max(min(int(args.split()[3]),255),0)
      b = max(min(int(args.split()[4]),255),0)
      self.pen.setColourRGBTuple((r,g,b)) #turn led on
    except:
      print "error"
      
  def do_goxy(self,args):
    """
    goxy
    """
    try:
      x = float(args.split()[0])
      y = float(args.split()[1])
      self.plotter.goTo(x,y)
    except:
      print "invalid arguments"
    

  def do_goxyrgb(self,args):
    """
    goxy
    """
    try:
      x = float(args.split()[0])
      y = float(args.split()[1])
      r = max(min(int(args.split()[2]),255),0)
      g = max(min(int(args.split()[3]),255),0)
      b = max(min(int(args.split()[4]),255),0)
    except:
      print "invalid arguments"
    self.pen.setColourRGBTuple((r,g,b)) #turn led on
    self.plotter.goTo(x,y)
    self.pen.setColourRGBTuple((0,0,0)) #turn led off

  def do_setusable(self,args):
    """
    Define the usable area. I have found 0.5 to be a good starting point
    """
    try:
      ratio =  float(args.split()[0])
    except:
      print "expecting a float from 0 < area <= 1"
    self.plotter.usableArea = max(min(ratio,1),0)
      
    
  def do_setorigin(self,args):
    """
    set current location as origin (top left = 0,0)
    """
    self.plotter.setOrigin()
  def do_setyoffset(self,args):
    """
    we can increas the y offset so that the entire plot is shifted downwards, so that image bottoms can be on the floor
    """
    try:
      offset = float(args.split()[0])
      self.plotter.yOffset = offset
    except:
      print "invalid args"
    
    

  def do_exit(self, line): return True


cint = cmdInterface().cmdloop()