#!/usr/bin/env python3

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


class Move:
    def __init__(self, planet):
        try:
            self._wheel_l = Wheel('outB')
            self._wheel_r = Wheel('outC')
            self._sensor = Sensor()
        except OSError:
            print("ERROR: Cannot find Motor/Sensor")
            self.maxbrightness = 0
            self.minbrightness = 0
            self.edgebrightness = 0
            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

    def calibrateBrightness(self):
        i = 0
        self.maxbrightness = self._camera.getbrightness()
        self.minbrightness = self._camera.getbrightness()
        self._wheel_l.speed_set(15)
        self._wheel_r.speed_set(-15)
        self._wheel_r.run()
        self._wheel_l.run()
        tempvalue = 0
        while(i < 500):
            self._camera.refresh()
            tempvalue = self._camera.getbrightness()
            if(tempvalue > self.maxbrightness):
                self.maxbrightness = tempvalue
            if(tempvalue < self.minbrightness):
                self.minbrightness = tempvalue
            i = i + 1
        self._wheel_l.stop()
        self._wheel_r.stop()
        self.edgebrightness = (self.maxbrightness + self.minbrightness)//2
        while(not (self._camera.getbrightness() == self.edgebrightness)):
            self._wheel_l.speed_set(15)
            self._wheel_r.speed_set(-15)
        self._wheel_l.stop()
        self._wheel_r.stop()

    '''
    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(self.edgebrightness//2)
        self._wheel_r.speed_set(self.edgebrightness//2)
        self._wheel_l.run()
        self._wheel_r.run()

        while (not self._sensor.bumperispressed() and self._sensor.lastcolor == None):
            self._camera.refresh()
            self._wheel_l.speed_set(self.maxbrightness//2 - self._sensor.getbrightness()//2)
            self._wheel_r.speed_set(self._sensor.getbrightness()//2)
            self._sensor.getcolor()
            if(not isknownstation):
                print(self._odometry.degree(self._wheel_l.getmovement(), self._wheel_r.getmovement(), 0))

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