# 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
"""


REF_CIRCLE_RADIUS = 1.25 # TODO measure in real arena!
REF_LINE_CROSSING_ANGLE = math.radians(50) # angle for selection of proper strip

LINE_OFFSET = 0.4 # asymetric line navigation
NUM_TRANSITION_STEPS = 5 # to switch from left/right orientation, also max limits
MAX_CIRCLE_OFFSET = 0.3 # for smooth transition, can be different



PATH_STRAIGHT = 'I'
PATH_TURN_RIGHT = 'R'
PATH_TURN_LEFT = 'L'


class StripsLocalisation:
  def __init__( self ):
    self.lastStripPose = None
    self.pathType = PATH_TURN_LEFT
    self.pathPose = None
    self.pathUpdated = False
    self.refCircle = None
    self.refLine = None
    self.countLR = 0
    self.crossing = False


  def diff2pathType( self, dx, dy, da ):

# 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

"""


REF_CIRCLE_RADIUS = 1.25 # TODO measure in real arena!
REF_LINE_CROSSING_ANGLE = math.radians(50) # angle for selection of proper strip

LINE_OFFSET = 0.4 # asymetric line navigation
NUM_TRANSITION_STEPS = 5 # to switch from left/right orientation, also max limits
MAX_CIRCLE_OFFSET = 0.3 # for smooth transition, can be different



PATH_STRAIGHT = 'I'
PATH_TURN_RIGHT = 'R'
PATH_TURN_LEFT = 'L'


class StripsLocalisation:
  def __init__( self ):
    self.lastStripPose = None
    self.pathType = PATH_TURN_LEFT
    self.pathPose = None
    self.pathUpdated = False
    self.refCircle = None
    self.refLine = None
    self.countLR = 0
    self.crossing = False


  def diff2pathType( self, dx, dy, da ):

    da = normalizeAnglePIPI(da)