from operator import add
import math

import utilities
reload(utilities)
from utilities import *

# Define new terminating behavior that goes to an (x, y) pose
class XYThDriver:
   forwardGain = 1.0
   rotationGain = 0.7
   angleEps = 0.02
   
    # poseFun is a function that can be called to return the current
    # pose (in whatever frame the goal is in)
    # motorOutput is a function to call that will make a motor command
    # to the robot
   def __init__(self, goalPose, poseFun, motorOutput, distEps):
      self.distEps = distEps
      self.goalPose = goalPose
      self.goalXY = goalPose[:-1]
      self.goalTheta = goalPose[-1]
      self.poseFun = poseFun
      self.motorOutput = motorOutput

   def start(self):
      pass

   def step(self):
      (px, py, ptheta) = self.poseFun()
      (gx, gy) = self.goalXY

      if nearPose2((px,py), (gx,gy), self.distEps):
         if nearAngle(ptheta, self.goalTheta, self.angleEps):
            # We're done
            self.motorOutput(0.0, 0.0)
         else:
            # XY is correct; rotate to final heading
            self.motorOutput (0.0, fixAnglePlusMinusPi(self.goalTheta-ptheta)*\
                              self.rotationGain)
      else:
         # Need to translate
         headingTheta = angleToPoint((px,py),(gx,gy))
         headingDeltaTheta = fixAnglePlusMinusPi(headingTheta - ptheta)
         if nearAngle(ptheta, headingTheta, self.angleEps):
            # Move forward
            r = math.sqrt((px - gx)**2 + (py - gy)**2)
            self.motorOutput(r*self.forwardGain, 0.0)
         else:
            # Rotate to point toward goal
            self.motorOutput(0.0, headingDeltaTheta * self.rotationGain)
   
   def done(self):
      currentPose = self.poseFun()
      return nearPose(currentPose, self.goalPose, self.distEps, self.angleEps)

# Define a new terminating behavior that goes to an (x,y,th) pose
# Terminate if blocked
class XYThDriverOA (XYThDriver):

    def __init__(self, goalPose, poseFun, motorOutput, sonarDistances, distEps):
        XYThDriver.__init__(self, goalPose, poseFun, motorOutput, distEps)
        self.sonarDistances = sonarDistances
        self.blockedThreshold = 0.1

    # Terminate if we're too close to any obstacle
    def done(self):
       minDist = min(self.sonarDistances())
       if minDist < self.blockedThreshold:
          print "Too close! ", minDist
       return XYThDriver.done(self) or minDist < self.blockedThreshold
     
     
   
            
   


