class PaVE:
  def __init__( self ):
    self.buf = ""

  def append( self, packet ):
    self.buf += packet


  def extract( self ):
    "return single packet (header, payload)"
    if not self.buf.startswith("PaVE"):
      if "PaVE" in self.buf:
        self.buf = self.buf[ self.buf.index("PaVE") : ]

    if len(self.buf) < 4+1+1+2+4:
      # at least struct of version and header and payload sizes must be ready
      return "",""
    
    if not self.buf.startswith( "PaVE" ):
      return "",""

    version, codec, headerSize, payloadSize = struct.unpack_from("BBHI", self.buf, 4 )
    if len(self.buf) < headerSize + payloadSize:
      return "",""

    ret = self.buf[:headerSize], self.buf[headerSize:headerSize+payloadSize]
    self.buf = self.buf[headerSize + payloadSize : ]
    return ret def isIFrame( header ):
  "return True if I-Frame"
  return struct.unpack_from("B", header, 30)[0] == 1 def frameNumber( header ):
  return struct.unpack_from("I", header, 20)[0] def timestamp( header ):
  return struct.unpack_from("I", header, 24)[0] def correctTimePeriod( value, ref, period=2048 ):
  "align drone time with video time"
  dt = value-ref
  while dt <= -period/2:
    dt += period
  while dt > period/2:
    dt -= period
  return ref+dt def frameEncodedWidth( header ):
  return struct.unpack_from("H", header, 12)[0] def frameEncodedHeight( header ):
  return struct.unpack_from("H", header, 14)[0] class SourceLogger:
  def __init__( self, sourceGet, filename ):
    self.sourceGet = sourceGet
    self.counter = 0
    self.prevData = None
    if self.sourceGet != None:
      self.file = open( filename, 'w' )
    else:
      self.file = open( filename )
      try:
        self.counterLimit = int(self.file.readline())
      except ValueError:
        # case when given device was not started
        print "EMPTY FILE!!!"
        self.counterLimit = 10000 # "infinity"


  def get( self ):
    if self.sourceGet != None:
      data = self.sourceGet()
      if data != None and data != self.prevData:
        self.file.write( str(self.counter) + "\n" )
        self.file.write( repr(data) + "\n" )
        self.file.flush()
        self.counter = 1
        self.prevData = data
        return self.prevData
    else:
      if self.counter >= self.counterLimit:
        self.counter = 1
        self.prevData = eval( self.file.readline() )
        nextCnt = self.file.readline()
        self.counterLimit = float('inf') if nextCnt == '' else int(nextCnt)
        return self.prevData
    self.counter += 1
    return None

  def generator( self ):
    assert( self.sourceGet is None )
    while 1:
      line = self.file.readline()
      if len(line) == 0:
        break
      self.prevData = eval( line )
      self.counterLimit = int(self.file.readline())
      yield self.prevData

  def __del__( self ):
    if self.sourceGet != None:
      self.file.write( str(self.counter) + "\n" )
    self.file.close() class ARDrone2:
  def __init__( self, replayLog=None, speed = 0.2, skipConfigure=False, metaLog=None, console=None ):
    self.replayLog = replayLog
    self.metaLog = metaLog
    self.sonar = None
    self.sonarUpdate = None
    self.sonarData = None
    self.sonarIndex = None # index of current measurement
    self.console = None
    if replayLog:
      self.filename = replayLog
      self.io = MyLogs( self.filename, metaLog=metaLog )
      if self.metaLog:
#        for line in metaLog:
#          print "LINE", line.strip()
#          if line.startswith("sonar:"):
#            self.sonarUpdate = SourceLogger( None, line.split()[1].strip() ).get
#            break
        self.console = SourceLogger( None, metaLog.getLog("console:") ).get
    else:
      if not os.path.isdir("logs"):
        os.mkdir("logs")
      self.filename = datetime.datetime.now().strftime("logs/navdata_%y%m%d_%H%M%S.log.gz")
      self.io = MySockets( self.filename, metaLog=metaLog )
      if console == None:
        if windows:
          console = myKbhit
        else:
          pygame.init()
          screen = pygame.display.set_mode((100,100))
          console = myPygame
      if self.metaLog:
#        sonarFilename = datetime.datetime.now().strftime("logs/sonar_src_%y%m%d_%H%M%S.log")
#        self.metaLog.write("sonar: "+sonarFilename+'\n' )
#        self.sonar = Sonar()
#        self.sonarUpdate = SourceLogger( self.sonar.data, sonarFilename ).get
#        self.sonar.start()
        consoleFilename = datetime.datetime.now().strftime("logs/console_src_%y%m%d_%H%M%S.log")
        self.metaLog.write("console: "+consoleFilename+'\n' )
        self.console = SourceLogger( console, consoleFilename ).get
      else:
        self.console = SourceLogger( console, 'metalog' ).get
    print self.filename
    self.speed = speed
    self.manualControl = False
    self.lastSeq = 0
    self.time = None
    self.ctrlAck = False # ACK_BIT is the 6th bit of ardrone_state
    self.ctrlState = None
    self.userEmergencyLanding = None
    self.battery = None
    self.acc = None # info about raw accelerometer readings
    self.pressure = None # raw data
    self.collision = False

    self.coord = (0,0,0)
    self.heading = 0.0
    self.headingOffset = None
    self.angleLR = 0.0
    self.angleFB = 0.0
    self.dt = 0 # time derivative
    self.vx, self.vy = None, None # speed in XY-axis (expect refactoring)
    self.visionTag = []
    self.altitudeData = None
    self.compass = None # single heading
    self.magneto = None # whole array
    self.gps = None  # whole raw data (for now)
    if not skipConfigure:
      self.configure()
    for i in xrange(20):
      self.update()
    assert( self.time != None )
    print "Battery", self.battery
    self.videoQueue1 = None
    self.videoProcess1 = None
    self.videoQueue2 = None
    self.videoProcess2 = None

  def update( self, cmd="AT*COMWDG=%i,\r" ):
    "take care of self.seq"
    if self.sonarUpdate:
      tmp = self.sonarUpdate()
      if tmp != None and len(tmp) >= 2:
        self.sonarIndex = tmp[0]
        if tmp[1] != 0:
          self.sonarData = tmp[1]/100.0 # conversion to meters
          viewlog.dumpSharps( (self.coord[0], self.coord[1], self.heading), [self.sonarData] )
        else:
          self.sonarData = 100.0 # far ...
#      print self.sonarData

    if not self.manualControl and self.console != None:
      self.manualControl = self.console()
      if self.manualControl:
        raise ManualControlException()
    self.lastSeq +=1
    data = self.io.update( cmd % self.lastSeq )
    self.parseNavData( data )
    return data # is it still necessary??

  def confirmedConfig( self, cmd ):
    "wait for configuration confirmation"
    assert( not self.ctrlAck ) # should be cleared on start and after each configuration
    self.update( "AT*CONFIG_IDS=%i,\"0a1b2c3d\",\"0a1b2c3d\",\"0a1b2c3d\"\r" )
    self.update( cmd )
    for i in xrange(200): # timeout by time??
      self.update()
      if self.ctrlAck:
        break
    assert( self.ctrlAck )
    self.update("AT*CTRL=%i,5,0\r") # ACK_CONTROL_MODE=5
    for i in xrange(100): # timeout by time??
      self.update()
      if not self.ctrlAck:
        print cmd
        break
    assert( not self.ctrlAck )


  def configure( self ):
    "initial configuration"
    self.confirmedConfig( "AT*CONFIG=%i,\"custom:application_id\",\"0a1b2c3d\"\r" ) # one day use some defines ...
    self.confirmedConfig( "AT*CONFIG=%i,\"custom:profile_id\",\"0a1b2c3d\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"custom:session_id\",\"0a1b2c3d\"\r" )
  
    self.confirmedConfig( "AT*CONFIG=%i,\"general:navdata_demo\",\"FALSE\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"control:outdoor\",\"TRUE\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"control:flight_without_shell\",\"FALSE\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"control:indoor_control_vz_max\",\"2000\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"control:altitude_max\",\"5000\"\r" )
    # CAD_TYPE_MULTIPLE_DETECTION_MODE = 10, CAD_TYPE_ORIENTED_COCARDE_BW=12, CAD_TYPE_VISION_V2 = 13
    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detect_type\",\"12\"\r") 
    # TAG_TYPE_SHELL_TAG=1, TAG_TYPE_ROUNDEL=2, TAG_TYPE_ORIENTED_ROUNDEL=4, TAG_TYPE_STRIPE=8
    # TAG_TYPE_SHELL_TAG_V2=32, TAG_TYPE_BLACK_ROUNDEL=128
    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_h\",\"0\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_v_hsync\",\"0\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_v\",\"128\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"detect:enemy_colors\",\"2\"\r" ) # ARDRONE_DETECTION_COLOR_ORANGE_YELLOW=2
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detect_type\",\"13\"\r") 
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_h\",\"32\"\r" )
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_v_hsync\",\"0\"\r" )
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:detections_select_v\",\"0\"\r" )
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:enemy_colors\",\"2\"\r" ) # ARDRONE_DETECTION_COLOR_ORANGE_YELLOW=2
#    self.confirmedConfig( "AT*CONFIG=%i,\"detect:enemy_without_shell\",\"0\"\r" )
    self.confirmedConfig( "AT*CONFIG=%i,\"video:video_channel\",\"0\"\r" ) # 0=VERTical, 1=HORIzontal view

  def setVideoChannel( self, front=True ):
    if front:
      self.confirmedConfig( "AT*CONFIG=%i,\"video:video_channel\",\"0\"\r" ) # 0=VERTical view
    else:
      self.confirmedConfig( "AT*CONFIG=%i,\"video:video_channel\",\"1\"\r" ) # 1=HORIzontal view

  def startVideo( self, packetProcessor=None, inputQueue=None, record=True, highResolution=True ):
    if self.videoQueue1 == None and self.videoProcess1 == None:
      if record:
        if highResolution:
          self.confirmedConfig( "AT*CONFIG=%i,\"video:video_codec\",\"130\"\r" ) # record MP4_360P_H264_720P_CODEC = 0x82,
        else:
          self.confirmedConfig( "AT*CONFIG=%i,\"video:video_codec\",\"136\"\r" ) # MP4_360P_H264_360P_CODEC = 0x88,
      else:
        if highResolution:
          self.confirmedConfig( "AT*CONFIG=%i,\"video:video_codec\",\"131\"\r" ) # H264_720P_CODEC = 0x83, 
        else:
          self.confirmedConfig( "AT*CONFIG=%i,\"video:video_codec\",\"129\"\r" ) # H264_360P_CODEC = 0x81,

      if self.replayLog == None:
        self.videoQueue1 = Queue()
        filename = datetime.datetime.now().strftime("logs/video_%y%m%d_%H%M%S.bin")
        if self.metaLog:
          self.metaLog.write("video: " + filename + "\n")
          self.metaLog.flush()
        if record:
          self.videoProcess1 = Process(target=logVideoStream, args=((HOST, VIDEO_PORT), filename, self.videoQueue1,))
        else:
          self.videoProcess1 = Process(target=logVideoStream, args=((HOST, VIDEO_PORT), filename, self.videoQueue1, \
            packetProcessor, inputQueue, False))
        self.videoProcess1.daemon = True
        self.videoProcess1.start()

        if record:
          self.videoQueue2 = Queue()
          filename = datetime.datetime.now().strftime("logs/video_rec_%y%m%d_%H%M%S.bin")
          if self.metaLog:
            self.metaLog.write("video_rec: " + filename + "\n")
            self.metaLog.flush()
          self.videoProcess2 = Process(target=logVideoStream, args=((HOST, VIDEO_RECORDER_PORT), filename, self.videoQueue2, \
              packetProcessor, inputQueue, True))
          self.videoProcess2.daemon = True
          self.videoProcess2.start()
      else:
        # replaying log
        if self.metaLog and record:
          viewlog.dumpVideo( self.metaLog.getLog("video_rec:") )

  def stopVideo( self ):
    if self.videoQueue1 != None and self.videoProcess1 != None:
      # TODO change configuration??
      # TODO mega assert to all video channels?
      print "sending video END ..."
      if self.replayLog == None:
        self.videoQueue1.put("The END")
        if self.videoQueue2:
          self.videoQueue2.put("The END")
        print "waiting for termination ..."
        for i in xrange(50):
          self.videoProcess1.join( timeout=0.1 )
          if not self.videoProcess1.is_alive():
            print "BREAK", i
            break
          self.update()
        else:
          print "TIMEOUT", i
        self.videoQueue1, self.videoProcess1 = None, None
      self.confirmedConfig( "AT*CONFIG=%i,\"video:video_codec\",\"129\"\r" )
      if self.replayLog == None and self.videoProcess2 != None:
        print "finishing record file"
        for i in xrange(300):
          self.videoProcess2.join(timeout=0.1) # i.e. wait 30sec for download and termination
          if not self.videoProcess2.is_alive():
            break
          self.update()
        self.videoQueue2, self.videoProcess2 = None, None
      print "DONE"

  def parseNavData( self, data ):
    "parse data and update ARDrone2 state"
    if len(data) > 16:
      header =  struct.unpack_from("IIII", data)
      offset = struct.calcsize("IIII")
      self.ctrlAck = header[1]>>6 & 1
#      self.userEmergencyLanding = header[1]>>16 & 1  # this is rather user input
      self.userEmergencyLanding = header[1]>>31 & 1  # Emergency landing : (0) no emergency, (1) emergency
      #print hex(header[1])
#      if header[1]>>19 & 1:
#        print "ERROR_STATE_EMERGENCY_ANGLE_OUT_OF_RANGE"
#      if header[1]>>22 & 1:
#        print "ARDRONE_CUTOUT_MASK"
      self.collision = False
      while True:
        tag, size =  struct.unpack_from("HH", data, offset)
        if tag == NAVDATA_CKS_TAG:
          break
        if tag == NAVDATA_DEMO_TAG:
          v = parseDemoTag( data, offset )
          self.ctrlState = v['ctrl_state']
          self.battery = v['battery']
          if self.headingOffset == None:
            self.headingOffset = math.radians(90.0) # used to be "math.radians(v['psi']/1000.)" but first data are not sufficiently reliable
          self.heading = self.headingOffset - math.radians(v['psi']/1000.)
          self.vx = v['vx']/1000.0
          self.vy = -v['vy']/1000.0
          dx = self.dt * self.vx
          dy = self.dt * self.vy
          c = math.cos(self.heading)
          s = math.sin(self.heading)
          self.coord = (self.coord[0]+c*dx-s*dy, self.coord[1]+s*dx+c*dy, v['altitude']/1000.0)
          assert v['vz']== 0.0
          self.angleLR = math.radians(v['phi']/1000.)
          self.angleFB = math.radians(v['theta']/1000.)
          if max(abs(self.angleLR), abs(self.angleFB)) > COLLISION_LIMIT_ANGLE:
            self.collision = True
#          if self.acc:
#            print self.ctrlState, '|\t%0.3f\t%0.3f\t%0.3f\t%d\t%d\t%d' % (v['theta']/1000., v['phi']/1000., v['psi']/1000., self.acc[0], self.acc[1], self.acc[2])
#          print self.ctrlState, "(%.1f, %.1f, %.1f)" % self.coord
          viewlog.dumpPose( (self.coord[0], self.coord[1], self.heading) )

        if tag == NAVDATA_TIME_TAG:
          t = parseTimeTag( data, offset )
          prev = self.time
          if self.time == None or self.time <= t:
            self.time = t
          else:
            self.time = t + (1<<11) # this handles only one 35min overflow
          if prev:
            self.dt = self.time - prev
            if int(prev*10) != int(self.time*10):
              viewlog.dumpSamples([(self.coord[0], self.coord[1], self.heading)])

        if tag == NAVDATA_RAW_MEASURES_TAG:
          self.acc = parseRawMeasuresTag( data, offset )
          if max(abs(self.acc[0]-2000), abs(self.acc[1]-2000)) > COLLISION_LIMIT_ACC:
            self.collision = True

        if tag == NAVDATA_VISION_DETECT_TAG:
          (countTypes, x, y, height, width, dist, oriAngle, cameraSource) = parseVisionDetectTag( data, offset )
          self.visionTag = []
          for i in xrange(countTypes[0]):
            if countTypes[i+1]:
              self.visionTag.append( (x[i], y[i], height[i], width[i], dist[i], oriAngle[i], cameraSource[i]) )
          if self.visionTag:
            viewlog.dumpBeacon( (self.coord[0], self.coord[1]), index=3 ) # well I did not properly implement the colors :( -> index=3

        if tag == NAVDATA_ALTITUDE_TAG:
          self.altitudeData = parseAltitudeTag( data, offset )

        if tag == NAVDATA_MAGNETO_TAG:
          self.magneto = parseMagnetoTag( data, offset )
          self.compass = math.radians(self.magneto[12])
          viewlog.dumpCompass( self.compass )

        if tag == NAVDATA_PRESSURE_RAW_TAG:
          self.pressure = parsePressureRawTag( data, offset )

        if tag == NAVDATA_GPS_TAG:
          self.gps = parseGPSTag( data, offset )

        offset += size


  def wait( self, seconds ):
    startTime = self.time
    assert( startTime != None )
    while self.time-startTime < seconds:
      self.update()

  def at_ref(seq, takeoff, emergency=False):
    """
    Basic behaviour of the drone: take-off/landing, emergency stop/reset)

    Parameters:
    seq -- sequence number
    takeoff -- True: Takeoff / False: Land
    emergency -- True: Turn of the engines
    """
    p = 0b10001010101000000000000000000
    if takeoff:
        p += 0b1000000000
    if emergency:
        p += 0b0100000000
    at("REF", seq, [p]) 

  def reset( self ):
    print "!!! RESET !!!"
    p = 0b10001010101000000000000000000
    p += 0b0100000000
    self.update( "AT*REF=%i,"+str(p)+'\r' )  # Emergency True
    p = 0b10001010101000000000000000000
    self.update( "AT*REF=%i,"+str(p)+'\r' )  # Emergency False

  def takeoff( self, timeout=10.0, enabledCorrections = False ):
    print "TRIM ..."
    self.update( "AT*FTRIM=%i,\r" )
    self.wait(0.1)
    print "Taking off ..."
    startTime = self.time
    assert( startTime != None )
    assert( self.ctrlState != None )
    p = 0b10001010101000000000000000000
    p += 0b1000000000    
    while self.time-startTime < timeout:
      self.update( "AT*REF=%i,"+str(p)+'\r' )
      if self.ctrlState in [3,4]: # CTRL_FLYING=3, CTRL_HOVERING=4
        break
      if enabledCorrections and self.time-startTime > 1.0 and self.coord[2] > 0.2:
        invCoord = relCoord( self.coord[0], self.coord[1], self.heading )
        vx, vy = 0, 0
        limit = 0.2
        if invCoord[0] > limit:
          vx = 0.1
        if invCoord[0] < -limit:
          vx = -0.1
        if invCoord[1] > limit:
          vy = 0.1
        if invCoord[1] < -limit:
          vy = -0.1
        print math.degrees(self.heading), self.coord, invCoord, vx, vy
        self.moveXYZA(vx,vy,0,0)
    print "state %d at %.1fs ... DONE" % ( self.ctrlState, self.time-startTime )


  def land( self, timeout = 10.0 ):
    print "Landing ...", self.coord
    startTime = self.time
    assert( startTime != None )
    assert( self.ctrlState != None )
    p = 0b10001010101000000000000000000
    while self.time-startTime < timeout:
      self.update( "AT*REF=%i,"+str(p)+'\r' )
      if self.ctrlState == 2: # CTRL_LANDED=2
        break
    print "Landing at %.1fs ... DONE" % ( self.time-startTime )

  def hover( self, time ):
    print "Hover..."
    startTime = self.time
    assert( startTime != None )
    while self.time-startTime < time:
      self.update("AT*PCMD=%i,0,0,0,0,0\r")

  def movePCMD( self, leftRight, frontBack, upDown, turn ):
    self.update("AT*PCMD=%i,1,"+ "%d,%d,%d,%d"%(f2i(leftRight),f2i(frontBack),f2i(upDown),f2i(turn)) + "\r")

  def moveForward( self, dist, timeout=3.0 ):
    print "Forward ...", dist
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( 0.0, -self.speed, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED (%.2f, %.2f, %.2f)" % self.coord
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def moveBackward( self, dist, timeout=3.0 ):
    print "Backward ...", dist
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( 0.0, self.speed, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED (%.2f, %.2f, %.2f)" % self.coord
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def moveLeft( self, dist, timeout=3.0 ):
    print "Left ...", dist
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( -self.speed, 0.0, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED (%.2f, %.2f, %.2f)" % self.coord
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def moveRight( self, dist, timeout=3.0 ):
    print "Right ...", dist
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( self.speed, 0.0, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED (%.2f, %.2f, %.2f)" % self.coord
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def moveUp( self, distance ):
    print "Up ...", distance
    time = distance*1.0 # no idea of scaling
    startTime = self.time
    while self.time-startTime < time:
      self.movePCMD( 0.0, 0.0, self.speed, 0.0 )

  def moveDown( self, distance ):
    print "Down ...", distance
    time = distance*1.0 # no idea of scaling
    startTime = self.time
    while self.time-startTime < time:
      self.movePCMD( 0.0, 0.0, -self.speed, 0.0 )
    print "timeout", self.time-startTime

  def turnLeft( self, angle ):
    print "Turn left ...", angle
    time = angle*1.0 # no idea of scaling
    startTime = self.time
    while self.time-startTime < time:
      self.movePCMD( 0.0, 0.0, 0.0, -self.speed )

  def turnRight( self, angle ):
    print "Turn right ...", angle
    time = angle*1.0 # no idea of scaling
    startTime = self.time
    while self.time-startTime < time:
      self.movePCMD( 0.0, 0.0, 0.0, self.speed )


  def moveDiagonal( self, dist, timeout=2.0 ):
    print "Diagonal FL ...", dist
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( -self.speed/2.0, -self.speed/2.0, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED"
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def moveXYZA( self, vx, vy, vz, va ):
    "make one step update with proper orientation"
    self.movePCMD( -vy, -vx, vz, -va )


  def navTagFB( self, timeout ):
    "make sure you are above tag in forward/backward direction"
    desiredScrY = 233 # TODO
    limit = 100 # TODO
    lostLimit = 100 # TODO
    lostCount = 0
    spd = self.speed/2.0
    self.update("AT*PCMD=%i,0,0,0,0,0\r") # one update with hover
    startTime = self.time
    while self.time-startTime < timeout:
      vision = self.visionTag
      if not vision:
        lostCount += 1
        if lostCount > lostLimit:
          print "LOST navTagFB", lostCount
          return False
        self.update("AT*PCMD=%i,0,0,0,0,0\r") # or repeat last command??
        continue
      lostCount = 0
      scrY = self.visionTag[0][1]
      print "diffY", desiredScrY - scrY
      if abs(desiredScrY - scrY) < limit:
        print vision
        self.update("AT*PCMD=%i,0,0,0,0,0\r")
        return True
      if scrY < desiredScrY:
        self.movePCMD( 0.0, -spd, 0.0, 0.0 ) # moveForward
      else:
        self.movePCMD( 0.0, spd, 0.0, 0.0 )
    print vision
    print "TIMEOUT"
    return None # ??

  def navTagLR( self, timeout ):
    "make sure you are above tag in left/right direction"
    desiredScrX = 300 # TODO
    limit = 100 # TODO
    lostLimit = 100 # TODO
    lostCount = 0
    spd = self.speed/2.0
    self.update("AT*PCMD=%i,0,0,0,0,0\r") # one update with hover
    startTime = self.time
    while self.time-startTime < timeout:
      vision = self.visionTag
      if not vision:
        lostCount += 1
        if lostCount > lostLimit:
          print "LOST navTagLR", lostCount
          return False
        self.update("AT*PCMD=%i,0,0,0,0,0\r") # or repeat last command??
        continue
      print vision
      lostCount = 0
      scrX = self.visionTag[0][0]
      print "diffX", desiredScrX - scrX
      if abs(desiredScrX - scrX) < limit:
        self.update("AT*PCMD=%i,0,0,0,0,0\r")
        return True
      if scrX < desiredScrX:
        print "LEFT"
        self.movePCMD( -spd, 0.0, 0.0, 0.0 ) # moveLeft
      else:
        print "RIGHT"
        self.movePCMD( spd, 0.0, 0.0, 0.0 )
    print "TIMEOUT"
    return None # ??

  def navTagAngle( self, timeout ):
    "make sure you are above tag in orientation"
    desiredScrAngle = 0.0 # or 270
    limit = 5 # TODO
    lostLimit = 100 # TODO
    lostCount = 0
    spd = self.speed/2.0
    self.update("AT*PCMD=%i,0,0,0,0,0\r") # one update with hover OR update only?
    startTime = self.time
    while self.time-startTime < timeout:
      vision = self.visionTag
      if not vision:
        lostCount += 1
        if lostCount > lostLimit:
          print "LOST navTagAngle", lostCount
          return False
        self.update("AT*PCMD=%i,0,0,0,0,0\r") # or repeat last command??
        continue
      lostCount = 0
      scrAngle = self.visionTag[0][5]
      diff = desiredScrAngle - scrAngle
      if diff > 180:
        diff -= 360
      elif diff < -180:
        diff += 360
#      print "diff", diff, limit, abs(diff) < limit
      if abs(diff) < limit:
        self.update("AT*PCMD=%i,0,0,0,0,0\r")
        print vision
        return True
      if diff < 0:
        self.movePCMD( 0.0, 0.0, 0.0, -spd ) # turnLeft
      else:
        self.movePCMD( 0.0, 0.0, 0.0, spd )
    print "TIMEOUT"
    print vision
    return None # ??





  def hoverRoundel0( self, time ):
    print "HoverRoundel..."
    startTime = self.time
#    self.confirmedConfig( "AT*CONFIG=%i,\"control:flying_mode\",\"2\"\r" ) # HOVER_ON_TOP_OF_ORIENTED_ROUNDEL=2
    self.confirmedConfig( "AT*CONFIG=%i,\"control:flying_mode\",\"3\"\r" ) # HOVER_ON_TOP_OF_ORIENTED_ROUNDEL=2
    assert( startTime != None )
    while self.time-startTime < time:
      self.update("AT*PCMD=%i,0,0,0,0,0\r")
    self.confirmedConfig( "AT*CONFIG=%i,\"control:flying_mode\",\"0\"\r" ) # FREE Flight

  def hoverRoundel( self, time ):
    print "HoverRoundel..."
    startTime = self.time
    while self.time-startTime < time:
      self.navTagFB(0.1)
      self.navTagLR(0.1)
      self.navTagAngle(0.1)


  def flyToRoundel( self, timeout ):
    print "Fly to roundel ..."
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( 0.0, -self.speed, 0.0, 0.0 )
      if self.visionTag:
        break
    print self.visionTag
#??    self.update("AT*PCMD=%i,0,0,0,0,0\r")
#    self.hoverRoundel( 1.0 )
    self.hover( 1.0 )

  def fly1meter( self, timeout ):
    print "Fly1meter"
    refX = self.coord[0]
    refY = self.coord[1]
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( 0.0, -self.speed, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > 1.0:
        print "DIST REACHED"
        break
    print distance( (refX, refY), self.coord ), self.time-startTime

  def flyToWall( self, vx, vy, timeout ):
    print "Fly to wall ..."
    startTime = self.time
    while self.time-startTime < timeout:
      self.movePCMD( -vy, -vx, 0.0, 0.0 )
      if self.collision:
        print "WALL HIT", math.degrees(self.angleLR), math.degrees(self.angleFB), self.acc
        break
    print "Fly END", self.time-startTime

  def flyToAltitude( self, altitude, timeout=3.0 ):
    print "Fly to altitude", altitude, "from", self.coord[2]
    startTime = self.time
    if self.coord[2] > altitude:
      while self.coord[2] > altitude and self.time-startTime < timeout:
        self.movePCMD( 0.0, 0.0, -self.speed, 0.0 )
    else:
      while self.coord[2] < altitude and self.time-startTime < timeout:
        self.movePCMD( 0.0, 0.0, self.speed, 0.0 ) # move up
    print "DONE at altitude", self.coord[2], "time", self.time-startTime

  def flyAtHeight( self, altitude, dist, timeout=20.0 ):
    print "flyAtHeight", altitude, dist, timeout 
    refX = self.coord[0]
    refY = self.coord[1]
    zTolerance = 0.5 # +/- no action
    startTime = self.time
    while self.time-startTime < timeout:
      altSonar, altVision = altitude, altitude
      if self.altitudeData != None:
        altVision = self.altitudeData[0]/1000.0
        altSonar = self.altitudeData[3]/1000.0
#      print altSonar, altVision
      if max(altSonar, altVision) < altitude-zTolerance:
        self.moveXYZA( self.speed/2.0, 0.0, self.speed, 0.0 )
      elif min(altSonar, altVision) > altitude+zTolerance:
        self.moveXYZA( self.speed/2.0, 0.0, -self.speed, 0.0 )
      else:
        self.moveXYZA( self.speed, 0.0, 0.0, 0.0 )
      if distance( (refX, refY), self.coord ) > dist:
        print "DIST REACHED"
        break


  def halt( self ):
    print "halt()", self.coord
    print "Battery:", self.battery
    if self.sonar != None:
      self.sonar.requestStop()
      self.sonar.join()
      self.sonar.close() class ManualControlException( Exception ):
  pass def manualControl( drone ):
  "manual control via keys"
  speed = 0.2 # or drone.speed?
  speedUpDown = 0.4
  while True:
    key = drone.console()
    if key:
      if key == ' ':
        return
      if key == ('\xe0', 'H'): # arrow up
         drone.moveXYZA( 0, 0, speedUpDown, 0 )
      elif key == ('\xe0', 'P'): # down
        drone.moveXYZA( 0, 0, -speedUpDown, 0 )
      elif key == ('\xe0', 'K'): # left
         drone.moveXYZA( 0, 0, 0, speed )
      elif key == ('\xe0', 'M'): # right
         drone.moveXYZA( 0, 0, 0, -speed )
      elif key == 'q': # forward
         drone.moveXYZA( speed, 0, 0, 0 )
      elif key == 'z': # backward
         drone.moveXYZA( -speed, 0, 0, 0 )
      elif key == 'p': # right
         drone.moveXYZA( 0, -speed, 0, 0 )
      elif key == 'i': # left
         drone.moveXYZA( 0, speed, 0, 0 )
      elif key == 'o': # hover
         drone.hover( 0.1 )
      else:
        return
    else:
      drone.update() def normalizeAnglePIPI( angle ):
  while angle < -math.pi:
    angle += 2*math.pi
  while angle > math.pi:
    angle -= 2*math.pi
  return angle  def distance( planar1, planar2 ):
  "distane two planar points"
  x = planar1[0] - planar2[0]
  y = planar1[1] - planar2[1]
  return math.hypot(x, y) class Pose:
  def __init__( self, x=0.0, y=0.0, heading=0.0 ):
    self.x, self.y, self.heading = x, y, heading

  def __str__( self ):
    return "(%.2f, %.2f, %d)" % (self.x, self.y, math.degrees(self.heading))

  def add( self, pose ):
    x = self.x + pose.x * math.cos( self.heading ) - pose.y * math.sin( self.heading )
    y = self.y + pose.x * math.sin( self.heading ) + pose.y * math.cos( self.heading )
    heading = self.heading + pose.heading
    return Pose(x, y, heading)

  def sub( self, pose ):
    heading = self.heading - pose.heading
    dx = self.x - pose.x
    dy = self.y - pose.y
    x = dx * math.cos( -pose.heading ) - dy * math.sin( -pose.heading )
    y = dx * math.sin( -pose.heading ) + dy * math.cos( -pose.heading )
    return Pose(x, y, heading)

  def coord( self ):
    return (self.x, self.y)

  def __iter__( self ):
    return self.iterator()

  def iterator( self ):
    yield self.x
    yield self.y
    yield self.heading def processAvoidGreen( frame, debug=False ):
  height, width, colors = frame.shape
  offset = 0
  stripWidth = 70
  limit = 120
  topLR = stripLeftRight( frame, offset, offset+stripWidth, limit=limit )
  bottomLR = stripLeftRight( frame, height-offset-stripWidth, height-offset, limit=limit )
  if debug:
    cv2.imshow('image', frame)
    saveIndexedImage( frame )
  return topLR, bottomLR
import sys
import datetime
import multiprocessing
import cv2
import math
import numpy as np
import viewlog
import cvideo
  import launcher
from pave import PaVE, isIFrame, frameNumber, timestamp, correctTimePeriod, frameEncodedWidth, frameEncodedHeight
from sourcelogger import SourceLogger
from ardrone2 import ARDrone2, ManualControlException, manualControl, normalizeAnglePIPI, distance
from line import Line
from pose import Pose
from green import processAvoidGreen
    header,payload = g_pave.extract()

g_queueResults = multiprocessing.Queue()

def getOrNone():
  if g_queueResults.empty():
    return None
  return g_queueResults.get()


###### --------------------------------------------------------------------------



# TODO for bottom cammera (!)
def project2plane( imgCoord, coord, height, heading, angleFB, angleLR ):
  FOW = math.radians(70)
  EPS = 0.0001
  x,y = imgCoord[0]-1280/2, 720/2-imgCoord[1]
  angleLR = -angleLR # we want to compensate the turn
  x,y = x*math.cos(angleLR)-y*math.sin(angleLR), y*math.cos(angleLR)+x*math.sin(angleLR)
  h = -x/1280*FOW + heading
  tilt = y/1280*FOW + angleFB
  if tilt > -EPS:
    return None # close to 0.0 AND projection behind drone
  dist = height/math.tan(-tilt)
  return (coord[0]+math.cos(h)*dist , coord[1]+math.sin(h)*dist)



class FieldRobotDrone( ARDrone2 ):
  def __init__( self, replayLog=None, speed = 0.2, skipConfigure=False, metaLog=None, console=None ):
    self.loggedVideoResult = None
    self.lastImageResult = None
    self.videoHighResolution = False
    ARDrone2.__init__( self, replayLog, speed, skipConfigure, metaLog, console )
    if replayLog == None:
      name = timeName( "logs/src_cv2_", "log" ) 
      metaLog.write("cv2: "+name+'\n' )
      self.loggedVideoResult = SourceLogger( getOrNone, name ).get
      self.startVideo( wrapper, g_queueResults, record=True, highResolution=self.videoHighResolution )
    else:
      assert metaLog
      self.loggedVideoResult = SourceLogger( None, metaLog.getLog("cv2:") ).get
      self.startVideo( record=True, highResolution=self.videoHighResolution )

  def update( self, cmd="AT*COMWDG=%i,\r" ):
    ARDrone2.update( self, cmd )
    if self.loggedVideoResult != None:
      self.lastImageResult = self.loggedVideoResult()


def stayAtPosition( drone, desiredHeight = 1.5, timeout = 10.0 ):
  maxControlGap = 0.0
  desiredSpeed = MAX_ALLOWED_SPEED
  refPoint = (0,0)
  foundTagTime = None
  searchSeq = [(0,0), (2,0), (0,-2), (-2,0), (0,2), (0,0)]
  startTime = drone.time
  sx,sy,sz,sa = 0,0,0,0
  stepTime = drone.time
  while drone.time < startTime + timeout:
      altitude = desiredHeight
      if drone.altitudeData != None:
        altVision = drone.altitudeData[0]/1000.0
        altSonar = drone.altitudeData[3]/1000.0
        altitude = (altSonar+altVision)/2.0
        # TODO selection based on history? panic when min/max too far??
        if abs(altSonar-altVision) > 0.5:
          print altSonar, altVision
          altitude = max( altSonar, altVision ) # sonar is 0.0 sometimes (no ECHO)

      sz = max( -0.2, min( 0.2, desiredHeight - altitude ))
      if altitude > 2.5:
        # wind and "out of control"
        sz = max( -0.5, min( 0.5, desiredHeight - altitude ))

      sx = 0.0 #max( 0, min( drone.speed, desiredSpeed - drone.vx ))

#        tiltCompensation = Pose(desiredHeight*oldAngles[0], desiredHeight*oldAngles[1], 0) # TODO real height?
#        print "FRAME", frameNumber/15, "[%.1f %.1f]" % (math.degrees(oldAngles[0]), math.degrees(oldAngles[1])),

#        if drone.battery < 10:
#          print "BATTERY LOW!", drone.battery


      # error definition ... if you substract that you get desired position or angle
      # error is taken from the path point of view, x-path direction, y-positive left, angle-anticlockwise
      errX, errY, errA = 0.0, 0.0, 0.0
#      if refLine:
#        errY = refLine.signedDistance( drone.coord )
#        errA = normalizeAnglePIPI( drone.heading - refLine.angle )

      if len(drone.visionTag) > 0:
        SCALE = 0.17/(2*74)
        tagX, tagY, tagDist = drone.visionTag[0][0], drone.visionTag[0][1], drone.visionTag[0][4]/100.0
        tiltCompensation = Pose(tagDist*drone.angleFB, tagDist*drone.angleLR, 0)
        pose = Pose(drone.coord[0], drone.coord[1], drone.heading).add(tiltCompensation)
        offset = Pose(tagDist*(480-tagY)*SCALE, tagDist*(tagX-640)*SCALE, 0.0)
        pose = pose.add( offset )
        refPoint = (pose.x, pose.y)
        if foundTagTime == None:
          print drone.visionTag
          print "%.2f\t%.2f\t%.2f\t%.1f\t%.1f" % (drone.time, tagDist*(tagY-480)*SCALE, tagDist*(tagX-640)*SCALE, math.degrees(drone.angleFB), math.degrees(drone.angleLR))
          print refPoint
        foundTagTime = drone.time
      else:
        if foundTagTime != None and drone.time - foundTagTime > 3.0:
          foundTagTime = None
          print "LOST TAG"
          searchSeq = [(x+drone.coord[0], y+drone.coord[1]) for (x,y) in [(0,0), (2,0), (0,-2), (-2,0), (0,2), (0,0)]]

      if foundTagTime == None and len(searchSeq) > 0 and drone.time - stepTime > 3.0:
        refPoint = searchSeq[0]
        searchSeq = searchSeq[1:]
        print "STEP", refPoint
        stepTime = drone.time

      if refPoint:
        pose = Pose(drone.coord[0], drone.coord[1], drone.heading)

        landPose = Pose( refPoint[0], refPoint[1], drone.heading )  # ignore heading for the moment