#!/usr/bin/env python3

from wheel import Wheel
from sensor import Sensor
from odometry import Odometry


def capat100(value):
    if(value > 100):
        return 100
    elif(value < -100):
        return -100
    else:
        return value


class Move:
    def __init__(self):
        try:
            self._wheel_l = Wheel('outB')
            self._wheel_r = Wheel('outC')
            self._sensor = Sensor()
        except OSError:
            print("ERROR: Cannot find Motor/Sensor")
        self.defaultspeed = 20
        self._odometry = Odometry()

    '''
    determine maximum and minimum brightness of lines/white space
    depending on environment lighting
    '''
    def _calibrate(self):
        self._sensor.calibrateRGB()
        self.calibrateBrightness()
        # pass



    '''
    Function to correct errors should the robot wander astray
    '''

    def _aligntoedge(self):
        pass

    '''
    uses odometry and the planet object to map edges that are connected to
    current node.

    Use either relative motor pos or timed driving to center on node.
    Next turn around and detect edges through drops in _sensor.getbrightness()
    '''
    def getstationedges(self):
        pass

    def turnto(self, direction):
        turnval = self._odometry.dir2ticks(direction)
        self._wheel_l.turnbyamount(turnval, self.defaultspeed)
        self._wheel_r.turnbyamount(-turnval, self.defaultspeed)

    def traversetonextstation(self, isknownstation):
        self._wheel_l.speed_set(self.edgebrightness//2)
        self._wheel_r.speed_set(self.edgebrightness//2)
        self._wheel_l.run()
        self._wheel_r.run()
        while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None):
            self._sensor.refresh()
            self._sensor.checkbumper()
            self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
            self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/2)+self.defaultspeed)
            self._sensor.getcolor()
            if(not isknownstation):
                self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))

        self._wheel_l.stop()
        self._wheel_r.stop()