# Path: rr_drone.py
# def processFrame( frame, debug=False ):
#   global g_mser
#   result = []
#   allHulls = []
#   selectedHulls = []
# #  for stripOffset in [ -100, 0, +100, +200 ]:
#   for stripOffset in [ +100 ]:
#     midY = frame.shape[0]/2+stripOffset
#     stripWidth = 120
#     if g_mser == None:
#       g_mser = cv2.MSER( _delta = 10, _min_area=100, _max_area=stripWidth*1000 )
#     imgStrip = frame[ midY:midY+stripWidth, 0:frame.shape[1] ]
#     b,g,r = cv2.split( imgStrip )
#     gray = b
#     contours = g_mser.detect(gray, None)
#     hulls = []
#     selected = None
#     for cnt in contours:
#       (x1,y1),(x2,y2) = np.amin( cnt, axis=0 ), np.amax( cnt, axis=0 )
#       if y1 == 0 and y2 == stripWidth-1: # i.e. whole strip
#         if x1 > 0 and x2 < frame.shape[1]-1:
#           hull = cv2.convexHull(cnt.reshape(-1, 1, 2))
#           for h in hull:
#             h[0][1] += midY
#           hulls.append( hull )
#           # select the one with the smallest area
#           if len(cnt) >= MIN_ROAD_AREA:
#             rect = rect2BLBRTRTL([(a[0][0],a[0][1]) for a in approx4pts( hull )] )
#             if rect != None:
#               bl,br,tr,tl = rect
#               tmpInt = Line(bl,tl).intersect( Line(br,tr) )
#               if tmpInt != None:
#                 p = tuple([int(x) for x in tmpInt])
#                 if br[0]-bl[0] > tr[0]-tl[0] and p[1] > 0 and p[1] < frame.shape[0]:
#                   # make sure that direction is forward and in the image
#                   result.append( rect )
# #                if selected == None or selected[0] > len(cnt):
# #                  selected = len(cnt), hull, rect
# #    if selected:
# #      result.append( selected[2] )
#     if debug:
#       allHulls.extend( hulls )
#       if selected != None:
#         selectedHulls.append( selected[1] )
#   
#   if debug:
#     cv2.polylines(frame, allHulls, 2, (0, 255, 0), 2)
#     if len(selectedHulls) > 0:
#       cv2.polylines(frame, selectedHulls, 2, (0, 0, 0), 2)
# #    for trapezoid in result:
# #      cv2.drawContours( frame,[np.int0(trapezoid)],0,(255,0,0),2)
# #    for trapezoid in result:
# #      bl,br,tr,tl = trapezoid
# #      p = tuple([int(x) for x in Line(bl,tl).intersect( Line(br,tr) )])
# #      cv2.circle( frame, p, 10, (0,255,128), 3 )
# #      navLine = trapezoid2line( trapezoid )
# #      if navLine:
# #        drawArrow(frame, navLine[0], navLine[1], (0,0,255), 4)
#     cv2.imshow('image', frame)
#     saveIndexedImage( frame )
#   return result


import sys
import os, math, pygame
import pave
import cv2
from pygame.locals import *
from rr_drone import processFrame

def scr1( point ) : return scr( point[0], point[1] )
#-------------------------------------------------

def getCombinedPose( pose, sensorPose ):
  x = pose[0] + sensorPose[0] * math.cos( pose[2] ) - sensorPose[1] * math.sin( pose[2] )
  y = pose[1] + sensorPose[0] * math.sin( pose[2] ) + sensorPose[1] * math.cos( pose[2] )
  heading = sensorPose[2] + pose[2]
  return (x, y, heading)

def loadVideoFrame( filename ):
  assert ":" in filename, filename
  frameNumber = int(filename.split(':')[-1])
  filename = ":".join(filename.split(":")[:-1])
  f = open(filename,"rb")
  proc = pave.PaVE()
  while True:
    packet = f.read(10000)
    if (packet) == 0:
      break
    proc.append( packet )
    header,payload = proc.extract()
    while payload:
      if pave.isIFrame( header ) and pave.frameNumber( header ) == frameNumber:
        tmpFile = open( "tmp.bin", "wb" )
        tmpFile.write( payload )
        tmpFile.flush()
        tmpFile.close()
        cap = cv2.VideoCapture( "tmp.bin" )
        ret, frame = cap.read()

# Path: rr_drone.py
# def processFrame( frame, debug=False ):
#   global g_mser
#   result = []
#   allHulls = []
#   selectedHulls = []
# #  for stripOffset in [ -100, 0, +100, +200 ]:
#   for stripOffset in [ +100 ]:
#     midY = frame.shape[0]/2+stripOffset
#     stripWidth = 120
#     if g_mser == None:
#       g_mser = cv2.MSER( _delta = 10, _min_area=100, _max_area=stripWidth*1000 )
#     imgStrip = frame[ midY:midY+stripWidth, 0:frame.shape[1] ]
#     b,g,r = cv2.split( imgStrip )
#     gray = b
#     contours = g_mser.detect(gray, None)
#     hulls = []
#     selected = None
#     for cnt in contours:
#       (x1,y1),(x2,y2) = np.amin( cnt, axis=0 ), np.amax( cnt, axis=0 )
#       if y1 == 0 and y2 == stripWidth-1: # i.e. whole strip
#         if x1 > 0 and x2 < frame.shape[1]-1:
#           hull = cv2.convexHull(cnt.reshape(-1, 1, 2))
#           for h in hull:
#             h[0][1] += midY
#           hulls.append( hull )
#           # select the one with the smallest area
#           if len(cnt) >= MIN_ROAD_AREA:
#             rect = rect2BLBRTRTL([(a[0][0],a[0][1]) for a in approx4pts( hull )] )
#             if rect != None:
#               bl,br,tr,tl = rect
#               tmpInt = Line(bl,tl).intersect( Line(br,tr) )
#               if tmpInt != None:
#                 p = tuple([int(x) for x in tmpInt])
#                 if br[0]-bl[0] > tr[0]-tl[0] and p[1] > 0 and p[1] < frame.shape[0]:
#                   # make sure that direction is forward and in the image
#                   result.append( rect )
# #                if selected == None or selected[0] > len(cnt):
# #                  selected = len(cnt), hull, rect
# #    if selected:
# #      result.append( selected[2] )
#     if debug:
#       allHulls.extend( hulls )
#       if selected != None:
#         selectedHulls.append( selected[1] )
#   
#   if debug:
#     cv2.polylines(frame, allHulls, 2, (0, 255, 0), 2)
#     if len(selectedHulls) > 0:
#       cv2.polylines(frame, selectedHulls, 2, (0, 0, 0), 2)
# #    for trapezoid in result:
# #      cv2.drawContours( frame,[np.int0(trapezoid)],0,(255,0,0),2)
# #    for trapezoid in result:
# #      bl,br,tr,tl = trapezoid
# #      p = tuple([int(x) for x in Line(bl,tl).intersect( Line(br,tr) )])
# #      cv2.circle( frame, p, 10, (0,255,128), 3 )
# #      navLine = trapezoid2line( trapezoid )
# #      if navLine:
# #        drawArrow(frame, navLine[0], navLine[1], (0,0,255), 4)
#     cv2.imshow('image', frame)
#     saveIndexedImage( frame )
#   return result


# Path: viewer.py
import sys
import os, math, pygame
import pave
import cv2
from pygame.locals import *
from rr_drone import processFrame


def scr1( point ) : return scr( point[0], point[1] )
#-------------------------------------------------

def getCombinedPose( pose, sensorPose ):
  x = pose[0] + sensorPose[0] * math.cos( pose[2] ) - sensorPose[1] * math.sin( pose[2] )
  y = pose[1] + sensorPose[0] * math.sin( pose[2] ) + sensorPose[1] * math.cos( pose[2] )
  heading = sensorPose[2] + pose[2]
  return (x, y, heading)

def loadVideoFrame( filename ):
  assert ":" in filename, filename
  frameNumber = int(filename.split(':')[-1])
  filename = ":".join(filename.split(":")[:-1])
  f = open(filename,"rb")
  proc = pave.PaVE()
  while True:
    packet = f.read(10000)
    if (packet) == 0:
      break
    proc.append( packet )
    header,payload = proc.extract()
    while payload:
      if pave.isIFrame( header ) and pave.frameNumber( header ) == frameNumber:
        tmpFile = open( "tmp.bin", "wb" )
        tmpFile.write( payload )
        tmpFile.flush()
        tmpFile.close()
        cap = cv2.VideoCapture( "tmp.bin" )
        ret, frame = cap.read()

        processFrame( frame, debug=True )  # hack!!