# Path: pave.py
# 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]
# 
# Path: pose.py
# 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


import sys
import cv2
import math
import numpy as np
from pave import PaVE, isIFrame, frameNumber, timestamp
from pose import Pose
    cv2.imshow('image', frame)
    # save image for simpler results review, angle is used as hash for search sub-sequence
    if g_filename:
      cv2.imwrite( g_filename, frame )
  return result

def filterRectangles( rects, minWidth=150, maxWidth=200 ):
  ret = []
  for (x,y),(w,h),a in rects:
    if w < h:
      w,h,a = h,w,a+90
    if w > 3*h and w >= minWidth and w <= maxWidth:
      ret.append( ((x,y,),(w,h),a) )
  return ret

def stripPose( rect, highResolution=True, scale=None ):
  "return relative pose of image rectangle"
  (x,y),(w,h),a = rect
  assert w > 3*h, (w,h)  # 30cm long, 5cm wide ... i.e. should be 6 times
  a = -a-90
  if a > 90:
    a -= 180
  if a < -90:
    a += 180
  if scale == None:
    if w >= 5*h: # it should be 6x, but width is more precise
      scale = 0.3/float(w)
    else:
      scale = 0.05/float(h)
  if highResolution:

# Path: pave.py
# 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]
# 
# Path: pose.py
# 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


# Path: airrace.py
import sys
import cv2
import math
import numpy as np
from pave import PaVE, isIFrame, frameNumber, timestamp
from pose import Pose

    cv2.imshow('image', frame)
    # save image for simpler results review, angle is used as hash for search sub-sequence
    if g_filename:
      cv2.imwrite( g_filename, frame )
  return result

def filterRectangles( rects, minWidth=150, maxWidth=200 ):
  ret = []
  for (x,y),(w,h),a in rects:
    if w < h:
      w,h,a = h,w,a+90
    if w > 3*h and w >= minWidth and w <= maxWidth:
      ret.append( ((x,y,),(w,h),a) )
  return ret

def stripPose( rect, highResolution=True, scale=None ):
  "return relative pose of image rectangle"
  (x,y),(w,h),a = rect
  assert w > 3*h, (w,h)  # 30cm long, 5cm wide ... i.e. should be 6 times
  a = -a-90
  if a > 90:
    a -= 180
  if a < -90:
    a += 180
  if scale == None:
    if w >= 5*h: # it should be 6x, but width is more precise
      scale = 0.3/float(w)
    else:
      scale = 0.05/float(h)
  if highResolution:

    return Pose( scale*(720/2-y), scale*(1280/2-x), math.radians( a ) )