class AutoRobot:
    def __init__(self, scanInterval=0.1):
        self.sensors = DistanceSensors()
        self.robot = Wheels()

        self.previousDistance = -1.0
        self.previousDirection = Direction.Stop
        self.stuckCount = 0

        # start distance scanning
        if not (self.sensors.StartScanner(scanInterval, True)):
            raise SensorException("Distance sensors not working")

    def GetDistanceToNextObstacle(self):
        '''
        Returns the remaining direction, distance in CMs in the current direction of travel
        Returns space in forward direction if stopped or rotating 
        '''
        distance = -1.0
        if self.robot.direction == Direction.Forward or self.robot.direction == Direction.Stop or \
            self.robot.direction == Direction.SpinLeft or self.robot.direction == Direction.SpinRight:
            distance = self.sensors.frontDistance[ServoDirection.Ahead]
        elif self.robot.direction == Direction.Reverse:
            distance = self.sensors.backDistance[ServoDirection.Ahead]
        elif self.robot.direction == Direction.ForwardRight:
            distance = self.sensors.frontDistance[ServoDirection.OffRight]
        elif self.robot.direction == Direction.ForwardLeft:
            distance = self.sensors.frontDistance[ServoDirection.OffLeft]
        elif self.robot.direction == Direction.ReverseLeft:
            distance = self.sensors.backDistance[ServoDirection.OffLeft]
        elif self.robot.direction == Direction.ReverseRight:
            distance = self.sensors.backDistance[ServoDirection.OffRight]
        elif self.robot.direction == Direction.MoveRight:
            distance = self.sensors.frontDistance[ServoDirection.Right]
        elif self.robot.direction == Direction.MoveLeft:
            distance = self.sensors.frontDistance[ServoDirection.Left]
        return self.robot.direction, distance

    def GetFurthestEnd(self):
        '''
        Returns direction, distance
        Work out if there is more space infront or behind and ignore sides
        '''
        if self.sensors.backDistance[
                ServoDirection.Ahead] > self.sensors.frontDistance[
                    ServoDirection.Ahead]:
            print("Furthest = Reverse",
                  self.sensors.backDistance[ServoDirection.Ahead])
            return Direction.Reverse, self.sensors.backDistance[
                ServoDirection.Ahead]
        else:
            print("Furthest = Forward",
                  self.sensors.frontDistance[ServoDirection.Ahead])
            return Direction.Forward, self.sensors.frontDistance[
                ServoDirection.Ahead]

    def GetMaxDistanceDirection(self):
        '''
        Returns servo end, servo direction, distance
        Returns the sensor direction with the most space and what that distance is
        '''
        maxRearDistance = max(self.sensors.backDistance.values())
        maxFrontDistance = max(self.sensors.frontDistance.values())

        if maxRearDistance > maxFrontDistance:
            res = [
                key for key in self.sensors.backDistance
                if self.sensors.backDistance[key] >= maxRearDistance
            ]
            if abs(self.sensors.backDistance[ServoDirection.Ahead] -
                   maxRearDistance) < 10:
                return ServoEnd.Back, ServoDirection.Ahead, self.sensors.backDistance[
                    ServoDirection.Ahead]
            else:
                return ServoEnd.Back, res[0], maxRearDistance
        else:
            res = [
                key for key in self.sensors.frontDistance
                if self.sensors.frontDistance[key] >= maxFrontDistance
            ]
            if abs(self.sensors.frontDistance[ServoDirection.Ahead] -
                   maxFrontDistance) < 10:
                return ServoEnd.Front, ServoDirection.Ahead, self.sensors.frontDistance[
                    ServoDirection.Ahead]
            else:
                return ServoEnd.Front, res[0], maxFrontDistance

    def GetMinDistanceDirection(self):
        '''   
        Returns servo end, servo direction, distance
        Returns the sensor direction with the most space and what that distance is
        '''
        minRearDistance = min(self.sensors.backDistance.values())
        minFrontDistance = min(self.sensors.frontDistance.values())

        if minRearDistance < minFrontDistance:
            res = [
                key for key in self.sensors.backDistance
                if self.sensors.backDistance[key] <= minRearDistance
            ]
            return ServoEnd.Back, res[0], minRearDistance
        else:
            res = [
                key for key in self.sensors.frontDistance
                if self.sensors.frontDistance[key] <= minFrontDistance
            ]
            return ServoEnd.Front, res[0], minFrontDistance

    def SetSpeedBasedOnDistance(self, distance):
        '''
        Set speed based on space infront of us
        Return the current speed
        '''
        if distance < 20.0:
            self.robot.Stop()
        elif distance < 40.0:
            self.robot.Speed(40)
        elif distance < 60.0:
            self.robot.Speed(50)
        elif distance < 100.0:
            self.robot.Speed(80)
        else:
            self.robot.Speed(100)
        return self.robot.robotspeed

    def RotateToBiggestSpace(self):
        ''' 
        Returns direction, distance in new direction of travel
        Rotates so either front or rear is pointing to biggests space.
        '''
        attempts = 5
        preferredDirection = self.GetMaxDistanceDirection()

        while attempts > 0:  # repeat until the front or back is pointing to the biggest space
            print("rotating, biggest space is ", preferredDirection)

            self.robot.Speed(70)

            # work out whether to spin right or left
            if (preferredDirection[0] == ServoEnd.Front and
                (preferredDirection[1] == ServoDirection.OffRight or preferredDirection[1] == ServoDirection.Right)) or \
                (preferredDirection[0] == ServoEnd.Back and
                (preferredDirection[1] == ServoDirection.OffLeft or preferredDirection[1] == ServoDirection.Left)):
                print("spin right")
                self.robot.SpinRight()
            else:
                print("spin left")
                self.robot.SpinLeft()

            if ServoDirection.OffLeft or ServoDirection.OffRight:
                time.sleep(0.5)  # rotate a bit
            else:
                time.sleep(1.0)  # rotate a lot
            self.robot.Stop()

            # reassess where the biggest space is
            preferredDirection = self.GetMaxDistanceDirection()

            # if the best direction is forward or reverse don't spin and return
            if preferredDirection[1] == ServoDirection.Ahead or \
                preferredDirection[1] == ServoDirection.OffLeft or \
                preferredDirection[1] == ServoDirection.OffRight:
                print("direction chosen", preferredDirection)
                if preferredDirection[0] == ServoEnd.Front:
                    return Direction.Forward, preferredDirection[2]
                else:
                    return Direction.Reverse, preferredDirection[2]

            # wait to get new set of distance readings
            time.sleep(1)
            attempts -= 1
        raise StuckException("cannot rotate out of trouble giving up")

    def AreWeStuck(self, direction, distance):
        '''
        Returns True if we haven't moved in the last four checks for being stuck
        '''
        if abs(
                distance - self.previousDistance
        ) < 1.0 and direction == self.previousDirection and direction != Direction.Stop:
            self.stuckCount += 1
            if self.stuckCount == 4:
                print("Stuck!")
                return True
        else:
            self.stuckCount = 0
        return False

    def UpdatePosition(self, direction, distance):
        '''
        Record our current location we we can determine later if we are stuck
        '''
        self.previousDirection = direction
        self.previousDistance = distance

    def GetNearestObstacleInDirectionOfTravel(self, currentDirection):
        '''
        Find out how far away we are from any obstacle directly infront of our direction of travel
        '''
        nearestObstacle = self.GetMinDistanceDirection()
        if nearestObstacle[0] == ServoEnd.Front:
            obstacleGeneralDirection = Direction.Forward
        else:
            obstacleGeneralDirection = Direction.Reverse
        if obstacleGeneralDirection != currentDirection:
            return 1000.0
        return nearestObstacle[2]
            robot.Backward()
        elif x == 's' or x == '0':
            robot.Stop()
        elif x == 'l':
            robot.SpinLeft()
        elif x == 'r':
            robot.SpinRight()
        elif x == 'mr':
            robot.MoveRight()
        elif x == 'ml':
            robot.MoveLeft()
        elif x == 'fr':
            robot.MoveForwardRight()
        elif x == 'fl':
            robot.MoveForwardLeft()
        elif x == 'br':
            robot.MoveBackwardRight()
        elif x == 'bl':
            robot.MoveBackwardLeft()
        elif x >= '4' and x <= '9':
            robot.Speed(int(x) * 10)
        elif x > '0' and x <= '3':
            robot.Speed(100)
            
        print("Front")
        print(sensors.frontDistance)
        print("back")
        print(sensors.backDistance)
finally:
    sensors.StopScanner()
    GPIO.cleanup()