#!/usr/bin/env python3

import ev3dev.ev3 as ev3
from wheel import Wheel
from sensor import Sensor


class Move:
    def __init__(self, planet):
        try:
            self._wheel_l = Wheel('outB')
            self._wheel_r = Wheel('outC')
            self._sensor = Sensor()
            self._bumper = ev3.TouchSensor()
        except:
            print("ERROR: Cannot find Motor/Sensor")

    '''
    determine maximum and minimum brightness of lines/white space
    depending on environment lighting
    '''
    def _calibrate(self):
        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):
        self._wheel_l.turnbyamount(500, 250)
        self._wheel_r.turnbyamount(-500, 250)

    def traversetonextstation(self, isknownstation):
        self._wheel_l.speed_set(24)
        self._wheel_r.speed_set(24)
        self._wheel_l.run()
        self._wheel_r.run()

        while(not (self._bumper.value() or self._sensor.isRed() or self._sensor.isBlue())):
            self._wheel_l.speed_set(48 - self._sensor.getbrightness())
            self._wheel_r.speed_set(self._sensor.getbrightness())
            if(isknownstation == False):
                pass #run odometry stuff here

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