# 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: ardrone2.py
# def normalizeAnglePIPI( angle ):
#   while angle < -math.pi:
#     angle += 2*math.pi
#   while angle > math.pi:
#     angle -= 2*math.pi
#   return angle 


from pose import Pose
from ardrone2 import normalizeAnglePIPI # TODO refactoring
from line import Line
import math

    if not self.pathUpdated:
      if len(frameStrips) == 1 and self.lastStripPose != None:
        fs = frameStrips[0]
        sPose = pose.add( fs )
        for lsp in self.lastStripPose:
          if self.isSameStrip( sPose, lsp ):
            # the self.pathType is the same only the pose is updated
            self.pathPose = sPose
            self.pathUpdated = True
        if not self.pathUpdated:
          for lsp in self.lastStripPose:
            print sPose.sub(lsp)

    if len(frameStrips) > 0:
      self.lastStripPose = []
      for fs in frameStrips:
        sPose = pose.add( fs )
        if verbose and self.lastStripPose != None:
          print [str(sPose.sub( lsp )) for lsp in self.lastStripPose]
        self.lastStripPose.append( sPose )

    if self.pathType == PATH_TURN_LEFT:
      self.countLR = max( -NUM_TRANSITION_STEPS, self.countLR - 1 )
    if self.pathType == PATH_TURN_RIGHT:
      self.countLR = min( NUM_TRANSITION_STEPS, self.countLR + 1 )

    if self.pathPose:
      sPose = self.pathPose
      if self.pathType == PATH_TURN_LEFT:

# 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: ardrone2.py
# def normalizeAnglePIPI( angle ):
#   while angle < -math.pi:
#     angle += 2*math.pi
#   while angle > math.pi:
#     angle -= 2*math.pi
#   return angle 


# Path: striploc.py
from pose import Pose
from ardrone2 import normalizeAnglePIPI # TODO refactoring
from line import Line
import math


    if not self.pathUpdated:
      if len(frameStrips) == 1 and self.lastStripPose != None:
        fs = frameStrips[0]
        sPose = pose.add( fs )
        for lsp in self.lastStripPose:
          if self.isSameStrip( sPose, lsp ):
            # the self.pathType is the same only the pose is updated
            self.pathPose = sPose
            self.pathUpdated = True
        if not self.pathUpdated:
          for lsp in self.lastStripPose:
            print sPose.sub(lsp)

    if len(frameStrips) > 0:
      self.lastStripPose = []
      for fs in frameStrips:
        sPose = pose.add( fs )
        if verbose and self.lastStripPose != None:
          print [str(sPose.sub( lsp )) for lsp in self.lastStripPose]
        self.lastStripPose.append( sPose )

    if self.pathType == PATH_TURN_LEFT:
      self.countLR = max( -NUM_TRANSITION_STEPS, self.countLR - 1 )
    if self.pathType == PATH_TURN_RIGHT:
      self.countLR = min( NUM_TRANSITION_STEPS, self.countLR + 1 )

    if self.pathPose:
      sPose = self.pathPose
      if self.pathType == PATH_TURN_LEFT:

        circCenter = sPose.add( Pose(0.0, REF_CIRCLE_RADIUS, 0 )).coord()